diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 9de8b01530..288eea3b22 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -167,7 +167,8 @@
/modules/gridmap/ @godotengine/3d-nodes
/modules/gridmap/doc_classes/ @godotengine/3d-nodes @godotengine/documentation
/modules/gridmap/icons/ @godotengine/3d-nodes @godotengine/usability
-/modules/navigation/ @godotengine/navigation
+/modules/navigation_2d/ @godotengine/navigation
+/modules/navigation_3d/ @godotengine/navigation
/modules/noise/ @godotengine/core
/modules/noise/doc_classes/ @godotengine/core @godotengine/documentation
/modules/noise/icons/ @godotengine/core @godotengine/usability
diff --git a/core/config/project_settings.cpp b/core/config/project_settings.cpp
index 9472c649c0..09c4608399 100644
--- a/core/config/project_settings.cpp
+++ b/core/config/project_settings.cpp
@@ -1631,6 +1631,17 @@ ProjectSettings::ProjectSettings() {
GLOBAL_DEF_INTERNAL("internationalization/locale/translations_pot_files", PackedStringArray());
GLOBAL_DEF_INTERNAL("internationalization/locale/translation_add_builtin_strings_to_pot", false);
+ GLOBAL_DEF("navigation/world/map_use_async_iterations", true);
+
+ GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true);
+ GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true);
+
+ GLOBAL_DEF("navigation/pathfinding/max_threads", 4);
+
+ GLOBAL_DEF("navigation/baking/use_crash_prevention_checks", true);
+ GLOBAL_DEF("navigation/baking/thread_model/baking_use_multiple_threads", true);
+ GLOBAL_DEF("navigation/baking/thread_model/baking_use_high_priority_threads", true);
+
ProjectSettings::get_singleton()->add_hidden_prefix("input/");
}
diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml
index 95db5591d0..f4625e827b 100644
--- a/doc/classes/NavigationServer2D.xml
+++ b/doc/classes/NavigationServer2D.xml
@@ -303,6 +303,13 @@
Returns all created navigation map [RID]s on the NavigationServer. This returns both 2D and 3D created navigation maps as there is technically no distinction between them.
+
+
+
+
+ Returns information about the current state of the NavigationServer. See [enum ProcessInfo] for a list of available states.
+
+
@@ -971,6 +978,13 @@
If [param enabled] is [code]true[/code], the navigation [param region] will use edge connections to connect with other navigation regions within proximity of the navigation map edge connection margin.
+
+
+
+
+ Control activation of this server.
+
+
@@ -1006,6 +1020,11 @@
+
+
+ Emitted when avoidance debug settings are changed. Only available in debug builds.
+
+
@@ -1018,4 +1037,36 @@
+
+
+ Constant to get the number of active navigation maps.
+
+
+ Constant to get the number of active navigation regions.
+
+
+ Constant to get the number of active navigation agents processing avoidance.
+
+
+ Constant to get the number of active navigation links.
+
+
+ Constant to get the number of navigation mesh polygons.
+
+
+ Constant to get the number of navigation mesh polygon edges.
+
+
+ Constant to get the number of navigation mesh polygon edges that were merged due to edge key overlap.
+
+
+ Constant to get the number of navigation mesh polygon edges that are considered connected by edge proximity.
+
+
+ Constant to get the number of navigation mesh polygon edges that could not be merged but may be still connected by edge proximity or with links.
+
+
+ Constant to get the number of active navigation obstacles.
+
+
diff --git a/doc/classes/Performance.xml b/doc/classes/Performance.xml
index 66078d2642..e882aa8e1d 100644
--- a/doc/classes/Performance.xml
+++ b/doc/classes/Performance.xml
@@ -195,34 +195,34 @@
Output latency of the [AudioServer]. Equivalent to calling [method AudioServer.get_output_latency], it is not recommended to call this every frame.
- Number of active navigation maps in the [NavigationServer3D]. This also includes the two empty default navigation maps created by World2D and World3D.
+ Number of active navigation maps in [NavigationServer2D] and [NavigationServer3D]. This also includes the two empty default navigation maps created by World2D and World3D.
- Number of active navigation regions in the [NavigationServer3D].
+ Number of active navigation regions in [NavigationServer2D] and [NavigationServer3D].
- Number of active navigation agents processing avoidance in the [NavigationServer3D].
+ Number of active navigation agents processing avoidance in [NavigationServer2D] and [NavigationServer3D].
- Number of active navigation links in the [NavigationServer3D].
+ Number of active navigation links in [NavigationServer2D] and [NavigationServer3D].
- Number of navigation mesh polygons in the [NavigationServer3D].
+ Number of navigation mesh polygons in [NavigationServer2D] and [NavigationServer3D].
- Number of navigation mesh polygon edges in the [NavigationServer3D].
+ Number of navigation mesh polygon edges in [NavigationServer2D] and [NavigationServer3D].
- Number of navigation mesh polygon edges that were merged due to edge key overlap in the [NavigationServer3D].
+ Number of navigation mesh polygon edges that were merged due to edge key overlap in [NavigationServer2D] and [NavigationServer3D].
- Number of polygon edges that are considered connected by edge proximity [NavigationServer3D].
+ Number of polygon edges that are considered connected by edge proximity [NavigationServer2D] and [NavigationServer3D].
- Number of navigation mesh polygon edges that could not be merged in the [NavigationServer3D]. The edges still may be connected by edge proximity or with links.
+ Number of navigation mesh polygon edges that could not be merged in [NavigationServer2D] and [NavigationServer3D]. The edges still may be connected by edge proximity or with links.
- Number of active navigation obstacles in the [NavigationServer3D].
+ Number of active navigation obstacles in the [NavigationServer2D] and [NavigationServer3D].
Number of pipeline compilations that were triggered by the 2D canvas renderer.
@@ -239,7 +239,67 @@
Number of pipeline compilations that were triggered to optimize the current scene. These compilations are done in the background and should not cause any stutters whatsoever.
-
+
+ Number of active navigation maps in the [NavigationServer2D]. This also includes the two empty default navigation maps created by World2D.
+
+
+ Number of active navigation regions in the [NavigationServer2D].
+
+
+ Number of active navigation agents processing avoidance in the [NavigationServer2D].
+
+
+ Number of active navigation links in the [NavigationServer2D].
+
+
+ Number of navigation mesh polygons in the [NavigationServer2D].
+
+
+ Number of navigation mesh polygon edges in the [NavigationServer2D].
+
+
+ Number of navigation mesh polygon edges that were merged due to edge key overlap in the [NavigationServer2D].
+
+
+ Number of polygon edges that are considered connected by edge proximity [NavigationServer2D].
+
+
+ Number of navigation mesh polygon edges that could not be merged in the [NavigationServer2D]. The edges still may be connected by edge proximity or with links.
+
+
+ Number of active navigation obstacles in the [NavigationServer2D].
+
+
+ Number of active navigation maps in the [NavigationServer3D]. This also includes the two empty default navigation maps created by World3D.
+
+
+ Number of active navigation regions in the [NavigationServer3D].
+
+
+ Number of active navigation agents processing avoidance in the [NavigationServer3D].
+
+
+ Number of active navigation links in the [NavigationServer3D].
+
+
+ Number of navigation mesh polygons in the [NavigationServer3D].
+
+
+ Number of navigation mesh polygon edges in the [NavigationServer3D].
+
+
+ Number of navigation mesh polygon edges that were merged due to edge key overlap in the [NavigationServer3D].
+
+
+ Number of polygon edges that are considered connected by edge proximity [NavigationServer3D].
+
+
+ Number of navigation mesh polygon edges that could not be merged in the [NavigationServer3D]. The edges still may be connected by edge proximity or with links.
+
+
+ Number of active navigation obstacles in the [NavigationServer3D].
+
+
Represents the size of the [enum Monitor] enum.
diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml
index c1083de9ec..5efb668228 100644
--- a/doc/classes/ProjectSettings.xml
+++ b/doc/classes/ProjectSettings.xml
@@ -688,31 +688,58 @@
When set to [code]true[/code], produces a warning when a varying is never used.
-
+
Color of the avoidance agents radius, visible when "Visible Avoidance" is enabled in the Debug menu.
-
+
If enabled, displays avoidance agents radius when "Visible Avoidance" is enabled in the Debug menu.
-
+
If enabled, displays avoidance obstacles radius when "Visible Avoidance" is enabled in the Debug menu.
-
+
If enabled, displays static avoidance obstacles when "Visible Avoidance" is enabled in the Debug menu.
-
+
Color of the avoidance obstacles radius, visible when "Visible Avoidance" is enabled in the Debug menu.
-
+
Color of the static avoidance obstacles edges when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
-
+
Color of the static avoidance obstacles edges when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
-
+
Color of the static avoidance obstacles faces when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
-
+
+ Color of the static avoidance obstacles faces when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ Color of the avoidance agents radius, visible when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ If enabled, displays avoidance agents radius when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ If enabled, displays avoidance obstacles radius when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ If enabled, displays static avoidance obstacles when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ Color of the avoidance obstacles radius, visible when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ Color of the static avoidance obstacles edges when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ Color of the static avoidance obstacles edges when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
+
+
+ Color of the static avoidance obstacles faces when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
+
+
Color of the static avoidance obstacles faces when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
@@ -727,58 +754,100 @@
Color of the collision shapes, visible when "Visible Collision Shapes" is enabled in the Debug menu.
-
+
Color to display enabled navigation agent paths when an agent has debug enabled.
-
+
Rasterized size (pixel) used to render navigation agent path points when an agent has debug enabled.
-
+
Color to display edge connections between navigation regions, visible when "Visible Navigation" is enabled in the Debug menu.
-
+
If enabled, displays navigation agent paths when an agent has debug enabled.
-
- If enabled, displays navigation agent paths through geometry when an agent has debug enabled.
-
-
+
If enabled, displays edge connections between navigation regions when "Visible Navigation" is enabled in the Debug menu.
-
- If enabled, displays edge connections between navigation regions through geometry when "Visible Navigation" is enabled in the Debug menu.
-
-
+
If enabled, displays navigation mesh polygon edges when "Visible Navigation" is enabled in the Debug menu.
-
- If enabled, displays navigation mesh polygon edges through geometry when "Visible Navigation" is enabled in the Debug menu.
-
-
+
If enabled, colorizes each navigation mesh polygon face with a random color when "Visible Navigation" is enabled in the Debug menu.
-
+
If enabled, displays navigation link connections when "Visible Navigation" is enabled in the Debug menu.
-
- If enabled, displays navigation link connections through geometry when "Visible Navigation" is enabled in the Debug menu.
-
-
+
Color to display enabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
-
+
Color to display disabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
-
+
Color to display enabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
-
+
Color to display disabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
-
+
Color to use to display navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
-
+
+ Color to use to display disabled navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
+ Color to display enabled navigation agent paths when an agent has debug enabled.
+
+
+ Rasterized size (pixel) used to render navigation agent path points when an agent has debug enabled.
+
+
+ Color to display edge connections between navigation regions, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, displays navigation agent paths when an agent has debug enabled.
+
+
+ If enabled, displays navigation agent paths through geometry when an agent has debug enabled.
+
+
+ If enabled, displays edge connections between navigation regions when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, displays edge connections between navigation regions through geometry when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, displays navigation mesh polygon edges when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, displays navigation mesh polygon edges through geometry when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, colorizes each navigation mesh polygon face with a random color when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, displays navigation link connections when "Visible Navigation" is enabled in the Debug menu.
+
+
+ If enabled, displays navigation link connections through geometry when "Visible Navigation" is enabled in the Debug menu.
+
+
+ Color to display enabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
+ Color to display disabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
+ Color to display enabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
+ Color to display disabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
+ Color to use to display navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
+
+
Color to use to display disabled navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index 689e898cc4..00fb834800 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -68,6 +68,7 @@
#include "scene/resources/portable_compressed_texture.h"
#include "scene/theme/theme_db.h"
#include "servers/display_server.h"
+#include "servers/navigation_server_2d.h"
#include "servers/navigation_server_3d.h"
#include "servers/rendering_server.h"
@@ -497,16 +498,25 @@ void EditorNode::_update_from_settings() {
ResourceImporterTexture::get_singleton()->update_imports();
#ifdef DEBUG_ENABLED
- NavigationServer3D::get_singleton()->set_debug_navigation_edge_connection_color(GLOBAL_GET("debug/shapes/navigation/edge_connection_color"));
- NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_color(GLOBAL_GET("debug/shapes/navigation/geometry_edge_color"));
- NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_color(GLOBAL_GET("debug/shapes/navigation/geometry_face_color"));
- NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(GLOBAL_GET("debug/shapes/navigation/geometry_edge_disabled_color"));
- NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(GLOBAL_GET("debug/shapes/navigation/geometry_face_disabled_color"));
- NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections(GLOBAL_GET("debug/shapes/navigation/enable_edge_connections"));
- NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections_xray(GLOBAL_GET("debug/shapes/navigation/enable_edge_connections_xray"));
- NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines(GLOBAL_GET("debug/shapes/navigation/enable_edge_lines"));
- NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines_xray(GLOBAL_GET("debug/shapes/navigation/enable_edge_lines_xray"));
- NavigationServer3D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(GLOBAL_GET("debug/shapes/navigation/enable_geometry_face_random_color"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_edge_connection_color(GLOBAL_GET("debug/shapes/navigation/2d/edge_connection_color"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_geometry_edge_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_edge_color"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_geometry_face_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_face_color"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_edge_disabled_color"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_face_disabled_color"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_enable_edge_connections(GLOBAL_GET("debug/shapes/navigation/2d/enable_edge_connections"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_enable_edge_lines(GLOBAL_GET("debug/shapes/navigation/2d/enable_edge_lines"));
+ NavigationServer2D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(GLOBAL_GET("debug/shapes/navigation/2d/enable_geometry_face_random_color"));
+
+ NavigationServer3D::get_singleton()->set_debug_navigation_edge_connection_color(GLOBAL_GET("debug/shapes/navigation/3d/edge_connection_color"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_edge_color"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_face_color"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_edge_disabled_color"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_face_disabled_color"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_connections"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections_xray(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_connections_xray"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_lines"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines_xray(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_lines_xray"));
+ NavigationServer3D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(GLOBAL_GET("debug/shapes/navigation/3d/enable_geometry_face_random_color"));
#endif // DEBUG_ENABLED
}
diff --git a/main/main.cpp b/main/main.cpp
index 66409e6240..de9179fc56 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -754,7 +754,7 @@ Error Main::test_setup() {
// Default theme will be initialized later, after modules and ScriptServer are ready.
initialize_theme_db();
- NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
+ NavigationServer3DManager::initialize_server();
NavigationServer2DManager::initialize_server();
register_scene_types();
@@ -839,7 +839,7 @@ void Main::test_cleanup() {
finalize_theme_db();
- NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
+ NavigationServer2DManager::finalize_server();
NavigationServer3DManager::finalize_server();
GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS);
@@ -3498,7 +3498,7 @@ Error Main::setup2(bool p_show_boot_logo) {
MAIN_PRINT("Main: Load Navigation");
- NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
+ NavigationServer3DManager::initialize_server();
NavigationServer2DManager::initialize_server();
register_scene_types();
@@ -4104,12 +4104,16 @@ int Main::start() {
}
if (debug_navigation) {
sml->set_debug_navigation_hint(true);
+ NavigationServer2D::get_singleton()->set_debug_navigation_enabled(true);
NavigationServer3D::get_singleton()->set_debug_navigation_enabled(true);
}
if (debug_avoidance) {
+ NavigationServer2D::get_singleton()->set_debug_avoidance_enabled(true);
NavigationServer3D::get_singleton()->set_debug_avoidance_enabled(true);
}
if (debug_navigation || debug_avoidance) {
+ NavigationServer2D::get_singleton()->set_active(true);
+ NavigationServer2D::get_singleton()->set_debug_enabled(true);
NavigationServer3D::get_singleton()->set_active(true);
NavigationServer3D::get_singleton()->set_debug_enabled(true);
}
@@ -4604,6 +4608,7 @@ bool Main::iteration() {
uint64_t navigation_begin = OS::get_singleton()->get_ticks_usec();
+ NavigationServer2D::get_singleton()->process(physics_step * time_scale);
NavigationServer3D::get_singleton()->process(physics_step * time_scale);
navigation_process_ticks = MAX(navigation_process_ticks, OS::get_singleton()->get_ticks_usec() - navigation_begin); // keep the largest one for reference
@@ -4846,7 +4851,7 @@ void Main::cleanup(bool p_force) {
finalize_theme_db();
// Before deinitializing server extensions, finalize servers which may be loaded as extensions.
- NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
+ NavigationServer2DManager::finalize_server();
NavigationServer3DManager::finalize_server();
finalize_physics();
diff --git a/main/performance.cpp b/main/performance.cpp
index 00ce64dd5d..d6f4264ce0 100644
--- a/main/performance.cpp
+++ b/main/performance.cpp
@@ -35,6 +35,7 @@
#include "scene/main/node.h"
#include "scene/main/scene_tree.h"
#include "servers/audio_server.h"
+#include "servers/navigation_server_2d.h"
#include "servers/navigation_server_3d.h"
#include "servers/rendering_server.h"
@@ -98,6 +99,26 @@ void Performance::_bind_methods() {
BIND_ENUM_CONSTANT(PIPELINE_COMPILATIONS_SURFACE);
BIND_ENUM_CONSTANT(PIPELINE_COMPILATIONS_DRAW);
BIND_ENUM_CONSTANT(PIPELINE_COMPILATIONS_SPECIALIZATION);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_ACTIVE_MAPS);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_REGION_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_AGENT_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_LINK_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_POLYGON_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_MERGE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_CONNECTION_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_FREE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_2D_OBSTACLE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_ACTIVE_MAPS);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_REGION_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_AGENT_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_LINK_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_POLYGON_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_MERGE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_CONNECTION_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_FREE_COUNT);
+ BIND_ENUM_CONSTANT(NAVIGATION_3D_OBSTACLE_COUNT);
BIND_ENUM_CONSTANT(MONITOR_MAX);
}
@@ -152,6 +173,26 @@ String Performance::get_monitor_name(Monitor p_monitor) const {
PNAME("pipeline/compilations_surface"),
PNAME("pipeline/compilations_draw"),
PNAME("pipeline/compilations_specialization"),
+ PNAME("navigation_2d/active_maps"),
+ PNAME("navigation_2d/regions"),
+ PNAME("navigation_2d/agents"),
+ PNAME("navigation_2d/links"),
+ PNAME("navigation_2d/polygons"),
+ PNAME("navigation_2d/edges"),
+ PNAME("navigation_2d/edges_merged"),
+ PNAME("navigation_2d/edges_connected"),
+ PNAME("navigation_2d/edges_free"),
+ PNAME("navigation_2d/obstacles"),
+ PNAME("navigation_2d/active_maps"),
+ PNAME("navigation_3d/regions"),
+ PNAME("navigation_3d/agents"),
+ PNAME("navigation_3d/links"),
+ PNAME("navigation_3d/polygons"),
+ PNAME("navigation_3d/edges"),
+ PNAME("navigation_3d/edges_merged"),
+ PNAME("navigation_3d/edges_connected"),
+ PNAME("navigation_3d/edges_free"),
+ PNAME("navigation_3d/obstacles"),
};
static_assert(std::size(names) == MONITOR_MAX);
@@ -237,25 +278,78 @@ double Performance::get_monitor(Monitor p_monitor) const {
case AUDIO_OUTPUT_LATENCY:
return AudioServer::get_singleton()->get_output_latency();
+
case NAVIGATION_ACTIVE_MAPS:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_ACTIVE_MAPS);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_ACTIVE_MAPS);
case NAVIGATION_REGION_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_REGION_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_REGION_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_REGION_COUNT);
case NAVIGATION_AGENT_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_AGENT_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_AGENT_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_AGENT_COUNT);
case NAVIGATION_LINK_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_LINK_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_LINK_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_LINK_COUNT);
case NAVIGATION_POLYGON_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_POLYGON_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_POLYGON_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_POLYGON_COUNT);
case NAVIGATION_EDGE_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_COUNT);
case NAVIGATION_EDGE_MERGE_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_MERGE_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_MERGE_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_MERGE_COUNT);
case NAVIGATION_EDGE_CONNECTION_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_CONNECTION_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_CONNECTION_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_CONNECTION_COUNT);
case NAVIGATION_EDGE_FREE_COUNT:
- return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_FREE_COUNT);
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_FREE_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_FREE_COUNT);
case NAVIGATION_OBSTACLE_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_OBSTACLE_COUNT) +
+ NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_OBSTACLE_COUNT);
+
+ case NAVIGATION_2D_ACTIVE_MAPS:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS);
+ case NAVIGATION_2D_REGION_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_REGION_COUNT);
+ case NAVIGATION_2D_AGENT_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_AGENT_COUNT);
+ case NAVIGATION_2D_LINK_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_LINK_COUNT);
+ case NAVIGATION_2D_POLYGON_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_POLYGON_COUNT);
+ case NAVIGATION_2D_EDGE_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_COUNT);
+ case NAVIGATION_2D_EDGE_MERGE_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_MERGE_COUNT);
+ case NAVIGATION_2D_EDGE_CONNECTION_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_CONNECTION_COUNT);
+ case NAVIGATION_2D_EDGE_FREE_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_FREE_COUNT);
+ case NAVIGATION_2D_OBSTACLE_COUNT:
+ return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_OBSTACLE_COUNT);
+
+ case NAVIGATION_3D_ACTIVE_MAPS:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_ACTIVE_MAPS);
+ case NAVIGATION_3D_REGION_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_REGION_COUNT);
+ case NAVIGATION_3D_AGENT_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_AGENT_COUNT);
+ case NAVIGATION_3D_LINK_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_LINK_COUNT);
+ case NAVIGATION_3D_POLYGON_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_POLYGON_COUNT);
+ case NAVIGATION_3D_EDGE_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_COUNT);
+ case NAVIGATION_3D_EDGE_MERGE_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_MERGE_COUNT);
+ case NAVIGATION_3D_EDGE_CONNECTION_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_CONNECTION_COUNT);
+ case NAVIGATION_3D_EDGE_FREE_COUNT:
+ return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_FREE_COUNT);
+ case NAVIGATION_3D_OBSTACLE_COUNT:
return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_OBSTACLE_COUNT);
default: {
@@ -308,6 +402,26 @@ Performance::MonitorType Performance::get_monitor_type(Monitor p_monitor) const
MONITOR_TYPE_QUANTITY,
MONITOR_TYPE_QUANTITY,
MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
+ MONITOR_TYPE_QUANTITY,
};
static_assert((sizeof(types) / sizeof(MonitorType)) == MONITOR_MAX);
diff --git a/main/performance.h b/main/performance.h
index 57ff3c368d..0fdac285cd 100644
--- a/main/performance.h
+++ b/main/performance.h
@@ -105,6 +105,26 @@ public:
PIPELINE_COMPILATIONS_SURFACE,
PIPELINE_COMPILATIONS_DRAW,
PIPELINE_COMPILATIONS_SPECIALIZATION,
+ NAVIGATION_2D_ACTIVE_MAPS,
+ NAVIGATION_2D_REGION_COUNT,
+ NAVIGATION_2D_AGENT_COUNT,
+ NAVIGATION_2D_LINK_COUNT,
+ NAVIGATION_2D_POLYGON_COUNT,
+ NAVIGATION_2D_EDGE_COUNT,
+ NAVIGATION_2D_EDGE_MERGE_COUNT,
+ NAVIGATION_2D_EDGE_CONNECTION_COUNT,
+ NAVIGATION_2D_EDGE_FREE_COUNT,
+ NAVIGATION_2D_OBSTACLE_COUNT,
+ NAVIGATION_3D_ACTIVE_MAPS,
+ NAVIGATION_3D_REGION_COUNT,
+ NAVIGATION_3D_AGENT_COUNT,
+ NAVIGATION_3D_LINK_COUNT,
+ NAVIGATION_3D_POLYGON_COUNT,
+ NAVIGATION_3D_EDGE_COUNT,
+ NAVIGATION_3D_EDGE_MERGE_COUNT,
+ NAVIGATION_3D_EDGE_CONNECTION_COUNT,
+ NAVIGATION_3D_EDGE_FREE_COUNT,
+ NAVIGATION_3D_OBSTACLE_COUNT,
MONITOR_MAX
};
diff --git a/misc/error_suppressions/tsan.txt b/misc/error_suppressions/tsan.txt
index 7545b06e27..9aa1d78753 100644
--- a/misc/error_suppressions/tsan.txt
+++ b/misc/error_suppressions/tsan.txt
@@ -4,4 +4,5 @@
deadlock:tests/core/templates/test_command_queue.h
deadlock:modules/text_server_adv/text_server_adv.cpp
deadlock:modules/text_server_fb/text_server_fb.cpp
-race:modules/navigation/nav_map_3d.cpp
+race:modules/navigation_2d/nav_map_2d.cpp
+race:modules/navigation_3d/nav_map_3d.cpp
diff --git a/modules/navigation/2d/godot_navigation_server_2d.cpp b/modules/navigation/2d/godot_navigation_server_2d.cpp
deleted file mode 100644
index 5fda299303..0000000000
--- a/modules/navigation/2d/godot_navigation_server_2d.cpp
+++ /dev/null
@@ -1,552 +0,0 @@
-/**************************************************************************/
-/* godot_navigation_server_2d.cpp */
-/**************************************************************************/
-/* This file is part of: */
-/* GODOT ENGINE */
-/* https://godotengine.org */
-/**************************************************************************/
-/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
-/* */
-/* Permission is hereby granted, free of charge, to any person obtaining */
-/* a copy of this software and associated documentation files (the */
-/* "Software"), to deal in the Software without restriction, including */
-/* without limitation the rights to use, copy, modify, merge, publish, */
-/* distribute, sublicense, and/or sell copies of the Software, and to */
-/* permit persons to whom the Software is furnished to do so, subject to */
-/* the following conditions: */
-/* */
-/* The above copyright notice and this permission notice shall be */
-/* included in all copies or substantial portions of the Software. */
-/* */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
-/**************************************************************************/
-
-#include "godot_navigation_server_2d.h"
-
-#ifdef CLIPPER2_ENABLED
-#include "nav_mesh_generator_2d.h"
-#endif // CLIPPER2_ENABLED
-
-#include "servers/navigation_server_3d.h"
-
-#define FORWARD_0(FUNC_NAME) \
- GodotNavigationServer2D::FUNC_NAME() { \
- return NavigationServer3D::get_singleton()->FUNC_NAME(); \
- }
-
-#define FORWARD_0_C(FUNC_NAME) \
- GodotNavigationServer2D::FUNC_NAME() \
- const { \
- return NavigationServer3D::get_singleton()->FUNC_NAME(); \
- }
-
-#define FORWARD_1(FUNC_NAME, T_0, D_0, CONV_0) \
- GodotNavigationServer2D::FUNC_NAME(T_0 D_0) { \
- return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
- }
-
-#define FORWARD_1_C(FUNC_NAME, T_0, D_0, CONV_0) \
- GodotNavigationServer2D::FUNC_NAME(T_0 D_0) \
- const { \
- return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
- }
-
-#define FORWARD_1_R_C(CONV_R, FUNC_NAME, T_0, D_0, CONV_0) \
- GodotNavigationServer2D::FUNC_NAME(T_0 D_0) \
- const { \
- return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0))); \
- }
-
-#define FORWARD_2(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
- GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) { \
- return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
- }
-
-#define FORWARD_2_C(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
- GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) \
- const { \
- return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
- }
-
-#define FORWARD_2_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
- GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) \
- const { \
- return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \
- }
-
-static RID rid_to_rid(const RID d) {
- return d;
-}
-
-static bool bool_to_bool(const bool d) {
- return d;
-}
-
-static int int_to_int(const int d) {
- return d;
-}
-
-static uint32_t uint32_to_uint32(const uint32_t d) {
- return d;
-}
-
-static real_t real_to_real(const real_t d) {
- return d;
-}
-
-static Vector3 v2_to_v3(const Vector2 d) {
- return Vector3(d.x, 0.0, d.y);
-}
-
-static Vector2 v3_to_v2(const Vector3 &d) {
- return Vector2(d.x, d.z);
-}
-
-static Vector vector_v2_to_v3(const Vector &d) {
- Vector nd;
- nd.resize(d.size());
- for (int i(0); i < nd.size(); i++) {
- nd.write[i] = v2_to_v3(d[i]);
- }
- return nd;
-}
-
-static Vector vector_v3_to_v2(const Vector &d) {
- Vector nd;
- nd.resize(d.size());
- for (int i(0); i < nd.size(); i++) {
- nd.write[i] = v3_to_v2(d[i]);
- }
- return nd;
-}
-
-static Transform3D trf2_to_trf3(const Transform2D &d) {
- Vector3 o(v2_to_v3(d.get_origin()));
- Basis b;
- b.rotate(Vector3(0, -1, 0), d.get_rotation());
- b.scale(v2_to_v3(d.get_scale()));
- return Transform3D(b, o);
-}
-
-static Transform2D trf3_to_trf2(const Transform3D &d) {
- Vector3 o = d.get_origin();
- Vector3 nx = d.xform(Vector3(1, 0, 0)) - o;
- Vector3 nz = d.xform(Vector3(0, 0, 1)) - o;
- return Transform2D(nx.x, nx.z, nz.x, nz.z, o.x, o.z);
-}
-
-static ObjectID id_to_id(const ObjectID &id) {
- return id;
-}
-
-static Callable callable_to_callable(const Callable &c) {
- return c;
-}
-
-static Ref poly_to_mesh(Ref d) {
- if (d.is_valid()) {
- return d->get_navigation_mesh();
- } else {
- return Ref();
- }
-}
-
-static Rect2 aabb_to_rect2(AABB aabb) {
- Rect2 rect2;
- rect2.position = Vector2(aabb.position.x, aabb.position.z);
- rect2.size = Vector2(aabb.size.x, aabb.size.z);
- return rect2;
-}
-
-void GodotNavigationServer2D::init() {
-#ifdef CLIPPER2_ENABLED
- navmesh_generator_2d = memnew(NavMeshGenerator2D);
- ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
- RWLockRead read_lock(geometry_parser_rwlock);
- navmesh_generator_2d->set_generator_parsers(generator_parsers);
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::sync() {
-#ifdef CLIPPER2_ENABLED
- if (navmesh_generator_2d) {
- navmesh_generator_2d->sync();
- }
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::finish() {
-#ifdef CLIPPER2_ENABLED
- if (navmesh_generator_2d) {
- navmesh_generator_2d->finish();
- memdelete(navmesh_generator_2d);
- navmesh_generator_2d = nullptr;
- }
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::parse_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
- ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
- ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
- ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
- ERR_FAIL_COND_MSG(!p_root_node->is_inside_tree(), "The root node needs to be inside the SceneTree.");
-
-#ifdef CLIPPER2_ENABLED
- ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
- NavMeshGenerator2D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::bake_from_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback) {
- ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
- ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
-
-#ifdef CLIPPER2_ENABLED
- ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
- NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback) {
- ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
- ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
-
-#ifdef CLIPPER2_ENABLED
- ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
- NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
-#endif // CLIPPER2_ENABLED
-}
-
-bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref p_navigation_polygon) const {
-#ifdef CLIPPER2_ENABLED
- return NavMeshGenerator2D::get_singleton()->is_baking(p_navigation_polygon);
-#else
- return false;
-#endif
-}
-
-Vector GodotNavigationServer2D::simplify_path(const Vector &p_path, real_t p_epsilon) {
- return vector_v3_to_v2(NavigationServer3D::get_singleton()->simplify_path(vector_v2_to_v3(p_path), p_epsilon));
-}
-
-GodotNavigationServer2D::GodotNavigationServer2D() {}
-
-GodotNavigationServer2D::~GodotNavigationServer2D() {}
-
-TypedArray FORWARD_0_C(get_maps);
-
-TypedArray FORWARD_1_C(map_get_links, RID, p_map, rid_to_rid);
-
-TypedArray FORWARD_1_C(map_get_regions, RID, p_map, rid_to_rid);
-
-TypedArray FORWARD_1_C(map_get_agents, RID, p_map, rid_to_rid);
-
-TypedArray FORWARD_1_C(map_get_obstacles, RID, p_map, rid_to_rid);
-
-RID FORWARD_1_C(region_get_map, RID, p_region, rid_to_rid);
-
-RID FORWARD_1_C(agent_get_map, RID, p_agent, rid_to_rid);
-
-RID FORWARD_0(map_create);
-
-void FORWARD_2(map_set_active, RID, p_map, bool, p_active, rid_to_rid, bool_to_bool);
-
-bool FORWARD_1_C(map_is_active, RID, p_map, rid_to_rid);
-
-void GodotNavigationServer2D::map_force_update(RID p_map) {
- NavigationServer3D::get_singleton()->map_force_update(p_map);
-}
-
-uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const {
- return NavigationServer3D::get_singleton()->map_get_iteration_id(p_map);
-}
-
-void GodotNavigationServer2D::map_set_use_async_iterations(RID p_map, bool p_enabled) {
- return NavigationServer3D::get_singleton()->map_set_use_async_iterations(p_map, p_enabled);
-}
-
-bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const {
- return NavigationServer3D::get_singleton()->map_get_use_async_iterations(p_map);
-}
-
-void FORWARD_2(map_set_cell_size, RID, p_map, real_t, p_cell_size, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(map_get_cell_size, RID, p_map, rid_to_rid);
-
-void FORWARD_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(map_get_use_edge_connections, RID, p_map, rid_to_rid);
-
-void FORWARD_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid);
-
-void FORWARD_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(map_get_link_connection_radius, RID, p_map, rid_to_rid);
-
-Vector GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
- return vector_v3_to_v2(NavigationServer3D::get_singleton()->map_get_path(p_map, v2_to_v3(p_origin), v2_to_v3(p_destination), p_optimize, p_navigation_layers));
-}
-
-Vector2 FORWARD_2_R_C(v3_to_v2, map_get_closest_point, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
-RID FORWARD_2_C(map_get_closest_point_owner, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
-
-Vector2 GodotNavigationServer2D::map_get_random_point(RID p_map, uint32_t p_naviation_layers, bool p_uniformly) const {
- Vector3 result = NavigationServer3D::get_singleton()->map_get_random_point(p_map, p_naviation_layers, p_uniformly);
- return v3_to_v2(result);
-}
-
-RID FORWARD_0(region_create);
-void FORWARD_2(region_set_enabled, RID, p_region, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(region_get_enabled, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(region_get_use_edge_connections, RID, p_region, rid_to_rid);
-
-void FORWARD_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(region_get_enter_cost, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(region_get_travel_cost, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id, rid_to_rid, id_to_id);
-ObjectID FORWARD_1_C(region_get_owner_id, RID, p_region, rid_to_rid);
-bool FORWARD_2_C(region_owns_point, RID, p_region, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
-
-void FORWARD_2(region_set_map, RID, p_region, RID, p_map, rid_to_rid, rid_to_rid);
-void FORWARD_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers, rid_to_rid, uint32_to_uint32);
-uint32_t FORWARD_1_C(region_get_navigation_layers, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_transform, RID, p_region, Transform2D, p_transform, rid_to_rid, trf2_to_trf3);
-
-Transform2D GodotNavigationServer2D::region_get_transform(RID p_region) const {
- return trf3_to_trf2(NavigationServer3D::get_singleton()->region_get_transform(p_region));
-}
-
-void GodotNavigationServer2D::region_set_navigation_polygon(RID p_region, Ref p_navigation_polygon) {
- NavigationServer3D::get_singleton()->region_set_navigation_mesh(p_region, poly_to_mesh(p_navigation_polygon));
-}
-
-int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid);
-Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
-Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
-
-Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const {
- Vector3 result = NavigationServer3D::get_singleton()->region_get_closest_point(p_region, v2_to_v3(p_point));
- return v3_to_v2(result);
-}
-
-Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
- Vector3 result = NavigationServer3D::get_singleton()->region_get_random_point(p_region, p_navigation_layers, p_uniformly);
- return v3_to_v2(result);
-}
-
-Rect2 GodotNavigationServer2D::region_get_bounds(RID p_region) const {
- AABB bounds = NavigationServer3D::get_singleton()->region_get_bounds(p_region);
- return aabb_to_rect2(bounds);
-}
-
-RID FORWARD_0(link_create);
-
-void FORWARD_2(link_set_map, RID, p_link, RID, p_map, rid_to_rid, rid_to_rid);
-RID FORWARD_1_C(link_get_map, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_enabled, RID, p_link, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(link_get_enabled, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(link_is_bidirectional, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers, rid_to_rid, uint32_to_uint32);
-uint32_t FORWARD_1_C(link_get_navigation_layers, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_start_position, RID, p_link, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 FORWARD_1_R_C(v3_to_v2, link_get_start_position, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_end_position, RID, p_link, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 FORWARD_1_R_C(v3_to_v2, link_get_end_position, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(link_get_enter_cost, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(link_get_travel_cost, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id, rid_to_rid, id_to_id);
-ObjectID FORWARD_1_C(link_get_owner_id, RID, p_link, rid_to_rid);
-
-RID GodotNavigationServer2D::agent_create() {
- RID agent = NavigationServer3D::get_singleton()->agent_create();
- return agent;
-}
-
-void FORWARD_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(agent_get_avoidance_enabled, RID, p_agent, rid_to_rid);
-void FORWARD_2(agent_set_map, RID, p_agent, RID, p_map, rid_to_rid, rid_to_rid);
-void FORWARD_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_dist, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_neighbor_distance(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_neighbor_distance(p_agent);
-}
-void FORWARD_2(agent_set_max_neighbors, RID, p_agent, int, p_count, rid_to_rid, int_to_int);
-int GodotNavigationServer2D::agent_get_max_neighbors(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_max_neighbors(p_agent);
-}
-void FORWARD_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_time_horizon_agents(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_time_horizon_agents(p_agent);
-}
-void FORWARD_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_time_horizon_obstacles(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_time_horizon_obstacles(p_agent);
-}
-void FORWARD_2(agent_set_radius, RID, p_agent, real_t, p_radius, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_radius(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_radius(p_agent);
-}
-void FORWARD_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_max_speed(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_max_speed(p_agent);
-}
-void FORWARD_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
-void FORWARD_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::agent_get_velocity(RID p_agent) const {
- return v3_to_v2(NavigationServer3D::get_singleton()->agent_get_velocity(p_agent));
-}
-void FORWARD_2(agent_set_position, RID, p_agent, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::agent_get_position(RID p_agent) const {
- return v3_to_v2(NavigationServer3D::get_singleton()->agent_get_position(p_agent));
-}
-bool FORWARD_1_C(agent_is_map_changed, RID, p_agent, rid_to_rid);
-void FORWARD_2(agent_set_paused, RID, p_agent, bool, p_paused, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(agent_get_paused, RID, p_agent, rid_to_rid);
-
-void GodotNavigationServer2D::free(RID p_object) {
- if (geometry_parser_owner.owns(p_object)) {
- RWLockWrite write_lock(geometry_parser_rwlock);
-
- NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
- ERR_FAIL_NULL(parser);
-
- generator_parsers.erase(parser);
-#ifndef CLIPPER2_ENABLED
- NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
-#endif
- geometry_parser_owner.free(parser->self);
- return;
- }
- NavigationServer3D::get_singleton()->free(p_object);
-}
-
-void FORWARD_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback, rid_to_rid, callable_to_callable);
-bool GodotNavigationServer2D::agent_has_avoidance_callback(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_has_avoidance_callback(p_agent);
-}
-
-void FORWARD_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
-uint32_t GodotNavigationServer2D::agent_get_avoidance_layers(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_avoidance_layers(p_agent);
-}
-void FORWARD_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask, rid_to_rid, uint32_to_uint32);
-uint32_t GodotNavigationServer2D::agent_get_avoidance_mask(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_avoidance_mask(p_agent);
-}
-void FORWARD_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_avoidance_priority(RID p_agent) const {
- return NavigationServer3D::get_singleton()->agent_get_avoidance_priority(p_agent);
-}
-
-RID GodotNavigationServer2D::obstacle_create() {
- RID obstacle = NavigationServer3D::get_singleton()->obstacle_create();
- return obstacle;
-}
-void FORWARD_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(obstacle_get_avoidance_enabled, RID, p_obstacle, rid_to_rid);
-void FORWARD_2(obstacle_set_map, RID, p_obstacle, RID, p_map, rid_to_rid, rid_to_rid);
-RID FORWARD_1_C(obstacle_get_map, RID, p_obstacle, rid_to_rid);
-void FORWARD_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(obstacle_get_paused, RID, p_obstacle, rid_to_rid);
-void FORWARD_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::obstacle_get_radius(RID p_obstacle) const {
- return NavigationServer3D::get_singleton()->obstacle_get_radius(p_obstacle);
-}
-void FORWARD_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::obstacle_get_velocity(RID p_obstacle) const {
- return v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_velocity(p_obstacle));
-}
-void FORWARD_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::obstacle_get_position(RID p_obstacle) const {
- return v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_position(p_obstacle));
-}
-void FORWARD_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
-uint32_t GodotNavigationServer2D::obstacle_get_avoidance_layers(RID p_obstacle) const {
- return NavigationServer3D::get_singleton()->obstacle_get_avoidance_layers(p_obstacle);
-}
-
-void GodotNavigationServer2D::obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) {
- NavigationServer3D::get_singleton()->obstacle_set_vertices(p_obstacle, vector_v2_to_v3(p_vertices));
-}
-Vector GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) const {
- return vector_v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_vertices(p_obstacle));
-}
-
-void GodotNavigationServer2D::query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) {
- ERR_FAIL_COND(p_query_parameters.is_null());
- ERR_FAIL_COND(p_query_result.is_null());
-
- Ref query_parameters;
- query_parameters.instantiate();
-
- query_parameters->set_map(p_query_parameters->get_map());
- query_parameters->set_start_position(v2_to_v3(p_query_parameters->get_start_position()));
- query_parameters->set_target_position(v2_to_v3(p_query_parameters->get_target_position()));
- query_parameters->set_navigation_layers(p_query_parameters->get_navigation_layers());
- query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
-
- switch (p_query_parameters->get_path_postprocessing()) {
- case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
- query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
- } break;
- case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
- query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED);
- } break;
- case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
- query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE);
- } break;
- default: {
- WARN_PRINT("No match for used PathPostProcessing - fallback to default");
- query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
- } break;
- }
-
- query_parameters->set_metadata_flags((int64_t)p_query_parameters->get_metadata_flags());
- query_parameters->set_simplify_path(p_query_parameters->get_simplify_path());
- query_parameters->set_simplify_epsilon(p_query_parameters->get_simplify_epsilon());
- query_parameters->set_excluded_regions(p_query_parameters->get_excluded_regions());
- query_parameters->set_included_regions(p_query_parameters->get_included_regions());
-
- Ref query_result;
- query_result.instantiate();
-
- NavigationServer3D::get_singleton()->query_path(query_parameters, query_result, p_callback);
-
- p_query_result->set_path(vector_v3_to_v2(query_result->get_path()));
- p_query_result->set_path_types(query_result->get_path_types());
- p_query_result->set_path_rids(query_result->get_path_rids());
- p_query_result->set_path_owner_ids(query_result->get_path_owner_ids());
-}
-
-RID GodotNavigationServer2D::source_geometry_parser_create() {
- RWLockWrite write_lock(geometry_parser_rwlock);
-
- RID rid = geometry_parser_owner.make_rid();
-
- NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
- parser->self = rid;
-
- generator_parsers.push_back(parser);
-#ifdef CLIPPER2_ENABLED
- NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
-#endif
- return rid;
-}
-
-void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
- RWLockWrite write_lock(geometry_parser_rwlock);
-
- NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
- ERR_FAIL_NULL(parser);
-
- parser->callback = p_callback;
-}
diff --git a/modules/navigation_2d/2d/godot_navigation_server_2d.cpp b/modules/navigation_2d/2d/godot_navigation_server_2d.cpp
new file mode 100644
index 0000000000..432541c6db
--- /dev/null
+++ b/modules/navigation_2d/2d/godot_navigation_server_2d.cpp
@@ -0,0 +1,1394 @@
+/**************************************************************************/
+/* godot_navigation_server_2d.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#include "godot_navigation_server_2d.h"
+
+#include "core/os/mutex.h"
+#include "scene/main/node.h"
+
+#ifdef CLIPPER2_ENABLED
+#include "nav_mesh_generator_2d.h"
+#endif // CLIPPER2_ENABLED
+
+#define COMMAND_1(F_NAME, T_0, D_0) \
+ struct MERGE(F_NAME, _command_2d) : public SetCommand2D { \
+ T_0 d_0; \
+ MERGE(F_NAME, _command_2d) \
+ (T_0 p_d_0) : \
+ d_0(p_d_0) {} \
+ virtual void exec(GodotNavigationServer2D *p_server) override { \
+ p_server->MERGE(_cmd_, F_NAME)(d_0); \
+ } \
+ }; \
+ void GodotNavigationServer2D::F_NAME(T_0 D_0) { \
+ auto cmd = memnew(MERGE(F_NAME, _command_2d)( \
+ D_0)); \
+ add_command(cmd); \
+ } \
+ void GodotNavigationServer2D::MERGE(_cmd_, F_NAME)(T_0 D_0)
+
+#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1) \
+ struct MERGE(F_NAME, _command_2d) : public SetCommand2D { \
+ T_0 d_0; \
+ T_1 d_1; \
+ MERGE(F_NAME, _command_2d) \
+ ( \
+ T_0 p_d_0, \
+ T_1 p_d_1) : \
+ d_0(p_d_0), \
+ d_1(p_d_1) {} \
+ virtual void exec(GodotNavigationServer2D *p_server) override { \
+ p_server->MERGE(_cmd_, F_NAME)(d_0, d_1); \
+ } \
+ }; \
+ void GodotNavigationServer2D::F_NAME(T_0 D_0, T_1 D_1) { \
+ auto cmd = memnew(MERGE(F_NAME, _command_2d)( \
+ D_0, \
+ D_1)); \
+ add_command(cmd); \
+ } \
+ void GodotNavigationServer2D::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+void GodotNavigationServer2D::init() {
+#ifdef CLIPPER2_ENABLED
+ navmesh_generator_2d = memnew(NavMeshGenerator2D);
+ ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
+ RWLockRead read_lock(geometry_parser_rwlock);
+ navmesh_generator_2d->set_generator_parsers(generator_parsers);
+#endif // CLIPPER2_ENABLED
+ // TODO
+}
+
+void GodotNavigationServer2D::sync() {
+#ifdef CLIPPER2_ENABLED
+ if (navmesh_generator_2d) {
+ navmesh_generator_2d->sync();
+ }
+#endif // CLIPPER2_ENABLED
+ // TODO
+}
+
+void GodotNavigationServer2D::finish() {
+#ifdef CLIPPER2_ENABLED
+ if (navmesh_generator_2d) {
+ navmesh_generator_2d->finish();
+ memdelete(navmesh_generator_2d);
+ navmesh_generator_2d = nullptr;
+ }
+#endif // CLIPPER2_ENABLED
+ // TODO
+}
+
+void GodotNavigationServer2D::parse_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
+ ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
+ ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
+ ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
+ ERR_FAIL_COND_MSG(!p_root_node->is_inside_tree(), "The root node needs to be inside the SceneTree.");
+
+#ifdef CLIPPER2_ENABLED
+ ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
+ NavMeshGenerator2D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
+#endif // CLIPPER2_ENABLED
+}
+
+void GodotNavigationServer2D::bake_from_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback) {
+ ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
+ ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
+
+#ifdef CLIPPER2_ENABLED
+ ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
+ NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
+#endif // CLIPPER2_ENABLED
+}
+
+void GodotNavigationServer2D::bake_from_source_geometry_data_async(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback) {
+ ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
+ ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
+
+#ifdef CLIPPER2_ENABLED
+ ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
+ NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
+#endif // CLIPPER2_ENABLED
+}
+
+bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref p_navigation_polygon) const {
+#ifdef CLIPPER2_ENABLED
+ return NavMeshGenerator2D::get_singleton()->is_baking(p_navigation_polygon);
+#else
+ return false;
+#endif
+}
+
+Vector GodotNavigationServer2D::simplify_path(const Vector &p_path, real_t p_epsilon) {
+ if (p_path.size() <= 2) {
+ return p_path;
+ }
+
+ p_epsilon = MAX(0.0, p_epsilon);
+
+ LocalVector source_path;
+ {
+ source_path.resize(p_path.size());
+ const Vector2 *r = p_path.ptr();
+ for (uint32_t i = 0; i < p_path.size(); i++) {
+ source_path[i] = r[i];
+ }
+ }
+
+ LocalVector simplified_path_indices = NavMeshQueries2D::get_simplified_path_indices(source_path, p_epsilon);
+
+ uint32_t index_count = simplified_path_indices.size();
+
+ Vector simplified_path;
+ {
+ simplified_path.resize(index_count);
+ Vector2 *w = simplified_path.ptrw();
+ const Vector2 *r = source_path.ptr();
+ for (uint32_t i = 0; i < index_count; i++) {
+ w[i] = r[simplified_path_indices[i]];
+ }
+ }
+
+ return simplified_path;
+}
+
+GodotNavigationServer2D::GodotNavigationServer2D() {}
+
+GodotNavigationServer2D::~GodotNavigationServer2D() {
+ flush_queries();
+}
+
+void GodotNavigationServer2D::add_command(SetCommand2D *p_command) {
+ MutexLock lock(commands_mutex);
+
+ commands.push_back(p_command);
+}
+
+TypedArray GodotNavigationServer2D::get_maps() const {
+ TypedArray all_map_rids;
+ List maps_owned;
+ map_owner.get_owned_list(&maps_owned);
+ if (maps_owned.size()) {
+ for (const RID &E : maps_owned) {
+ all_map_rids.push_back(E);
+ }
+ }
+ return all_map_rids;
+}
+
+TypedArray GodotNavigationServer2D::map_get_links(RID p_map) const {
+ TypedArray link_rids;
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, link_rids);
+
+ const LocalVector &links = map->get_links();
+ link_rids.resize(links.size());
+
+ for (uint32_t i = 0; i < links.size(); i++) {
+ link_rids[i] = links[i]->get_self();
+ }
+ return link_rids;
+}
+
+TypedArray GodotNavigationServer2D::map_get_regions(RID p_map) const {
+ TypedArray regions_rids;
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, regions_rids);
+
+ const LocalVector ®ions = map->get_regions();
+ regions_rids.resize(regions.size());
+
+ for (uint32_t i = 0; i < regions.size(); i++) {
+ regions_rids[i] = regions[i]->get_self();
+ }
+ return regions_rids;
+}
+
+TypedArray GodotNavigationServer2D::map_get_agents(RID p_map) const {
+ TypedArray agents_rids;
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, agents_rids);
+
+ const LocalVector &agents = map->get_agents();
+ agents_rids.resize(agents.size());
+
+ for (uint32_t i = 0; i < agents.size(); i++) {
+ agents_rids[i] = agents[i]->get_self();
+ }
+ return agents_rids;
+}
+
+TypedArray GodotNavigationServer2D::map_get_obstacles(RID p_map) const {
+ TypedArray obstacles_rids;
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, obstacles_rids);
+ const LocalVector obstacles = map->get_obstacles();
+ obstacles_rids.resize(obstacles.size());
+ for (uint32_t i = 0; i < obstacles.size(); i++) {
+ obstacles_rids[i] = obstacles[i]->get_self();
+ }
+ return obstacles_rids;
+}
+
+RID GodotNavigationServer2D::region_get_map(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, RID());
+
+ if (region->get_map()) {
+ return region->get_map()->get_self();
+ }
+ return RID();
+}
+
+RID GodotNavigationServer2D::agent_get_map(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, RID());
+
+ if (agent->get_map()) {
+ return agent->get_map()->get_self();
+ }
+ return RID();
+}
+
+RID GodotNavigationServer2D::map_create() {
+ MutexLock lock(operations_mutex);
+
+ RID rid = map_owner.make_rid();
+ NavMap2D *map = map_owner.get_or_null(rid);
+ map->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+
+ if (p_active) {
+ if (!map_is_active(p_map)) {
+ active_maps.push_back(map);
+ active_maps_iteration_id.push_back(map->get_iteration_id());
+ }
+ } else {
+ int map_index = active_maps.find(map);
+ ERR_FAIL_COND(map_index < 0);
+ active_maps.remove_at(map_index);
+ active_maps_iteration_id.remove_at(map_index);
+ }
+}
+
+bool GodotNavigationServer2D::map_is_active(RID p_map) const {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, false);
+
+ return active_maps.has(map);
+}
+
+void GodotNavigationServer2D::map_force_update(RID p_map) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+
+ flush_queries();
+
+ map->sync();
+}
+
+uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, 0);
+
+ return map->get_iteration_id();
+}
+
+COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+ map->set_use_async_iterations(p_enabled);
+}
+
+bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, false);
+
+ return map->get_use_async_iterations();
+}
+
+COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+
+ map->set_cell_size(p_cell_size);
+}
+
+real_t GodotNavigationServer2D::map_get_cell_size(RID p_map) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, 0);
+
+ return map->get_cell_size();
+}
+
+COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+
+ map->set_use_edge_connections(p_enabled);
+}
+
+bool GodotNavigationServer2D::map_get_use_edge_connections(RID p_map) const {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, false);
+
+ return map->get_use_edge_connections();
+}
+
+COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+
+ map->set_edge_connection_margin(p_connection_margin);
+}
+
+real_t GodotNavigationServer2D::map_get_edge_connection_margin(RID p_map) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, 0);
+
+ return map->get_edge_connection_margin();
+}
+
+COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius) {
+ NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL(map);
+
+ map->set_link_connection_radius(p_connection_radius);
+}
+
+real_t GodotNavigationServer2D::map_get_link_connection_radius(RID p_map) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, 0);
+
+ return map->get_link_connection_radius();
+}
+
+Vector GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, Vector());
+
+ Ref query_parameters;
+ query_parameters.instantiate();
+
+ query_parameters->set_map(p_map);
+ query_parameters->set_start_position(p_origin);
+ query_parameters->set_target_position(p_destination);
+ query_parameters->set_navigation_layers(p_navigation_layers);
+ query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters2D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
+ query_parameters->set_path_postprocessing(NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
+ if (!p_optimize) {
+ query_parameters->set_path_postprocessing(NavigationPathQueryParameters2D::PATH_POSTPROCESSING_EDGECENTERED);
+ }
+
+ Ref query_result;
+ query_result.instantiate();
+
+ query_path(query_parameters, query_result);
+
+ return query_result->get_path();
+}
+
+Vector2 GodotNavigationServer2D::map_get_closest_point(RID p_map, const Vector2 &p_point) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, Vector2());
+
+ return map->get_closest_point(p_point);
+}
+
+RID GodotNavigationServer2D::map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, RID());
+
+ return map->get_closest_point_owner(p_point);
+}
+
+Vector2 GodotNavigationServer2D::map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const {
+ const NavMap2D *map = map_owner.get_or_null(p_map);
+ ERR_FAIL_NULL_V(map, Vector2());
+
+ return map->get_random_point(p_navigation_layers, p_uniformly);
+}
+
+RID GodotNavigationServer2D::region_create() {
+ MutexLock lock(operations_mutex);
+
+ RID rid = region_owner.make_rid();
+ NavRegion2D *reg = region_owner.get_or_null(rid);
+ reg->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ region->set_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::region_get_enabled(RID p_region) const {
+ const NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, false);
+
+ return region->get_enabled();
+}
+
+COMMAND_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ region->set_use_edge_connections(p_enabled);
+}
+
+bool GodotNavigationServer2D::region_get_use_edge_connections(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, false);
+
+ return region->get_use_edge_connections();
+}
+
+COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+ ERR_FAIL_COND(p_enter_cost < 0.0);
+
+ region->set_enter_cost(p_enter_cost);
+}
+
+real_t GodotNavigationServer2D::region_get_enter_cost(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, 0);
+
+ return region->get_enter_cost();
+}
+
+COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+ ERR_FAIL_COND(p_travel_cost < 0.0);
+
+ region->set_travel_cost(p_travel_cost);
+}
+
+real_t GodotNavigationServer2D::region_get_travel_cost(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, 0);
+
+ return region->get_travel_cost();
+}
+
+COMMAND_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ region->set_owner_id(p_owner_id);
+}
+
+ObjectID GodotNavigationServer2D::region_get_owner_id(RID p_region) const {
+ const NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, ObjectID());
+
+ return region->get_owner_id();
+}
+
+bool GodotNavigationServer2D::region_owns_point(RID p_region, const Vector2 &p_point) const {
+ const NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, false);
+
+ if (region->get_map()) {
+ RID closest_point_owner = map_get_closest_point_owner(region->get_map()->get_self(), p_point);
+ return closest_point_owner == region->get_self();
+ }
+ return false;
+}
+
+COMMAND_2(region_set_map, RID, p_region, RID, p_map) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ NavMap2D *map = map_owner.get_or_null(p_map);
+
+ region->set_map(map);
+}
+
+COMMAND_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ region->set_navigation_layers(p_navigation_layers);
+}
+
+uint32_t GodotNavigationServer2D::region_get_navigation_layers(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, 0);
+
+ return region->get_navigation_layers();
+}
+
+COMMAND_2(region_set_transform, RID, p_region, Transform2D, p_transform) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ region->set_transform(p_transform);
+}
+
+Transform2D GodotNavigationServer2D::region_get_transform(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, Transform2D());
+
+ return region->get_transform();
+}
+
+void GodotNavigationServer2D::region_set_navigation_polygon(RID p_region, Ref p_navigation_polygon) {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL(region);
+
+ region->set_navigation_polygon(p_navigation_polygon);
+}
+
+int GodotNavigationServer2D::region_get_connections_count(RID p_region) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, 0);
+ NavMap2D *map = region->get_map();
+ if (map) {
+ return map->get_region_connections_count(region);
+ }
+ return 0;
+}
+
+Vector2 GodotNavigationServer2D::region_get_connection_pathway_start(RID p_region, int p_connection_id) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, Vector2());
+ NavMap2D *map = region->get_map();
+ if (map) {
+ return map->get_region_connection_pathway_start(region, p_connection_id);
+ }
+ return Vector2();
+}
+
+Vector2 GodotNavigationServer2D::region_get_connection_pathway_end(RID p_region, int p_connection_id) const {
+ NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, Vector2());
+ NavMap2D *map = region->get_map();
+ if (map) {
+ return map->get_region_connection_pathway_end(region, p_connection_id);
+ }
+ return Vector2();
+}
+
+Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const {
+ const NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, Vector2());
+
+ return region->get_closest_point_info(p_point).point;
+}
+
+Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
+ const NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, Vector2());
+
+ return region->get_random_point(p_navigation_layers, p_uniformly);
+}
+
+Rect2 GodotNavigationServer2D::region_get_bounds(RID p_region) const {
+ const NavRegion2D *region = region_owner.get_or_null(p_region);
+ ERR_FAIL_NULL_V(region, Rect2());
+
+ return region->get_bounds();
+}
+
+RID GodotNavigationServer2D::link_create() {
+ MutexLock lock(operations_mutex);
+
+ RID rid = link_owner.make_rid();
+ NavLink2D *link = link_owner.get_or_null(rid);
+ link->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(link_set_map, RID, p_link, RID, p_map) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ NavMap2D *map = map_owner.get_or_null(p_map);
+
+ link->set_map(map);
+}
+
+RID GodotNavigationServer2D::link_get_map(const RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, RID());
+
+ if (link->get_map()) {
+ return link->get_map()->get_self();
+ }
+ return RID();
+}
+
+COMMAND_2(link_set_enabled, RID, p_link, bool, p_enabled) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::link_get_enabled(RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, false);
+
+ return link->get_enabled();
+}
+
+COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_bidirectional(p_bidirectional);
+}
+
+bool GodotNavigationServer2D::link_is_bidirectional(RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, false);
+
+ return link->is_bidirectional();
+}
+
+COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_navigation_layers(p_navigation_layers);
+}
+
+uint32_t GodotNavigationServer2D::link_get_navigation_layers(const RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, 0);
+
+ return link->get_navigation_layers();
+}
+
+COMMAND_2(link_set_start_position, RID, p_link, Vector2, p_position) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_start_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::link_get_start_position(RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, Vector2());
+
+ return link->get_start_position();
+}
+
+COMMAND_2(link_set_end_position, RID, p_link, Vector2, p_position) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_end_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::link_get_end_position(RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, Vector2());
+
+ return link->get_end_position();
+}
+
+COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_enter_cost(p_enter_cost);
+}
+
+real_t GodotNavigationServer2D::link_get_enter_cost(const RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, 0);
+
+ return link->get_enter_cost();
+}
+
+COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_travel_cost(p_travel_cost);
+}
+
+real_t GodotNavigationServer2D::link_get_travel_cost(const RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, 0);
+
+ return link->get_travel_cost();
+}
+
+COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id) {
+ NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL(link);
+
+ link->set_owner_id(p_owner_id);
+}
+
+ObjectID GodotNavigationServer2D::link_get_owner_id(RID p_link) const {
+ const NavLink2D *link = link_owner.get_or_null(p_link);
+ ERR_FAIL_NULL_V(link, ObjectID());
+
+ return link->get_owner_id();
+}
+
+RID GodotNavigationServer2D::agent_create() {
+ MutexLock lock(operations_mutex);
+
+ RID rid = agent_owner.make_rid();
+ NavAgent2D *agent = agent_owner.get_or_null(rid);
+ agent->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_avoidance_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::agent_get_avoidance_enabled(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, false);
+
+ return agent->is_avoidance_enabled();
+}
+
+COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ NavMap2D *map = map_owner.get_or_null(p_map);
+
+ agent->set_map(map);
+}
+
+COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_neighbor_distance(p_distance);
+}
+
+real_t GodotNavigationServer2D::agent_get_neighbor_distance(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_neighbor_distance();
+}
+
+COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_max_neighbors(p_count);
+}
+
+int GodotNavigationServer2D::agent_get_max_neighbors(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_max_neighbors();
+}
+
+COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon) {
+ ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizon must be positive.");
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_time_horizon_agents(p_time_horizon);
+}
+
+real_t GodotNavigationServer2D::agent_get_time_horizon_agents(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_time_horizon_agents();
+}
+
+COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon) {
+ ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizon must be positive.");
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_time_horizon_obstacles(p_time_horizon);
+}
+
+real_t GodotNavigationServer2D::agent_get_time_horizon_obstacles(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_time_horizon_obstacles();
+}
+
+COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) {
+ ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_radius(p_radius);
+}
+
+real_t GodotNavigationServer2D::agent_get_radius(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_radius();
+}
+
+COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
+ ERR_FAIL_COND_MSG(p_max_speed < 0.0, "Max speed must be positive.");
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_max_speed(p_max_speed);
+}
+
+real_t GodotNavigationServer2D::agent_get_max_speed(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_max_speed();
+}
+
+COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_velocity_forced(p_velocity);
+}
+
+COMMAND_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_velocity(p_velocity);
+}
+
+Vector2 GodotNavigationServer2D::agent_get_velocity(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, Vector2());
+
+ return agent->get_velocity();
+}
+
+COMMAND_2(agent_set_position, RID, p_agent, Vector2, p_position) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::agent_get_position(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, Vector2());
+
+ return agent->get_position();
+}
+
+bool GodotNavigationServer2D::agent_is_map_changed(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, false);
+
+ return agent->is_map_changed();
+}
+
+COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_avoidance_callback(p_callback);
+
+ if (agent->get_map()) {
+ if (p_callback.is_valid()) {
+ agent->get_map()->set_agent_as_controlled(agent);
+ } else {
+ agent->get_map()->remove_agent_as_controlled(agent);
+ }
+ }
+}
+
+bool GodotNavigationServer2D::agent_has_avoidance_callback(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, false);
+
+ return agent->has_avoidance_callback();
+}
+
+COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+ agent->set_avoidance_layers(p_layers);
+}
+
+uint32_t GodotNavigationServer2D::agent_get_avoidance_layers(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_avoidance_layers();
+}
+
+COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+ agent->set_avoidance_mask(p_mask);
+}
+
+uint32_t GodotNavigationServer2D::agent_get_avoidance_mask(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_avoidance_mask();
+}
+
+COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority) {
+ ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+ ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+ agent->set_avoidance_priority(p_priority);
+}
+
+real_t GodotNavigationServer2D::agent_get_avoidance_priority(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, 0);
+
+ return agent->get_avoidance_priority();
+}
+
+COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL(agent);
+
+ agent->set_paused(p_paused);
+}
+
+bool GodotNavigationServer2D::agent_get_paused(RID p_agent) const {
+ NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+ ERR_FAIL_NULL_V(agent, false);
+
+ return agent->get_paused();
+}
+
+RID GodotNavigationServer2D::obstacle_create() {
+ MutexLock lock(operations_mutex);
+
+ RID rid = obstacle_owner.make_rid();
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(rid);
+ obstacle->set_self(rid);
+
+ RID agent_rid = agent_owner.make_rid();
+ NavAgent2D *agent = agent_owner.get_or_null(agent_rid);
+ agent->set_self(agent_rid);
+
+ obstacle->set_agent(agent);
+
+ return rid;
+}
+
+COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+
+ obstacle->set_avoidance_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::obstacle_get_avoidance_enabled(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, false);
+
+ return obstacle->is_avoidance_enabled();
+}
+
+COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+
+ NavMap2D *map = map_owner.get_or_null(p_map);
+
+ obstacle->set_map(map);
+}
+
+RID GodotNavigationServer2D::obstacle_get_map(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, RID());
+ if (obstacle->get_map()) {
+ return obstacle->get_map()->get_self();
+ }
+ return RID();
+}
+
+COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+
+ obstacle->set_paused(p_paused);
+}
+
+bool GodotNavigationServer2D::obstacle_get_paused(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, false);
+
+ return obstacle->get_paused();
+}
+
+COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius) {
+ ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+
+ obstacle->set_radius(p_radius);
+}
+
+real_t GodotNavigationServer2D::obstacle_get_radius(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, 0);
+
+ return obstacle->get_radius();
+}
+
+COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+
+ obstacle->set_velocity(p_velocity);
+}
+
+Vector2 GodotNavigationServer2D::obstacle_get_velocity(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, Vector2());
+
+ return obstacle->get_velocity();
+}
+
+COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+ obstacle->set_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::obstacle_get_position(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, Vector2());
+
+ return obstacle->get_position();
+}
+
+COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+ obstacle->set_avoidance_layers(p_layers);
+}
+
+uint32_t GodotNavigationServer2D::obstacle_get_avoidance_layers(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, 0);
+
+ return obstacle->get_avoidance_layers();
+}
+
+void GodotNavigationServer2D::obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL(obstacle);
+ obstacle->set_vertices(p_vertices);
+}
+
+Vector GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) const {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+ ERR_FAIL_NULL_V(obstacle, Vector());
+
+ return obstacle->get_vertices();
+}
+
+void GodotNavigationServer2D::flush_queries() {
+ // In c++ we can't be sure that this is performed in the main thread
+ // even with mutable functions.
+ MutexLock lock(commands_mutex);
+ MutexLock lock2(operations_mutex);
+
+ for (SetCommand2D *command : commands) {
+ command->exec(this);
+ memdelete(command);
+ }
+ commands.clear();
+}
+
+COMMAND_1(free, RID, p_object) {
+ if (geometry_parser_owner.owns(p_object)) {
+ RWLockWrite write_lock(geometry_parser_rwlock);
+
+ NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
+ ERR_FAIL_NULL(parser);
+
+ generator_parsers.erase(parser);
+#ifndef CLIPPER2_ENABLED
+ NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
+#endif
+ geometry_parser_owner.free(parser->self);
+ return;
+ }
+ if (map_owner.owns(p_object)) {
+ NavMap2D *map = map_owner.get_or_null(p_object);
+
+ // Removes any assigned region.
+ for (NavRegion2D *region : map->get_regions()) {
+ map->remove_region(region);
+ region->set_map(nullptr);
+ }
+
+ // Removes any assigned links.
+ for (NavLink2D *link : map->get_links()) {
+ map->remove_link(link);
+ link->set_map(nullptr);
+ }
+
+ // Remove any assigned agent.
+ for (NavAgent2D *agent : map->get_agents()) {
+ map->remove_agent(agent);
+ agent->set_map(nullptr);
+ }
+
+ // Remove any assigned obstacles.
+ for (NavObstacle2D *obstacle : map->get_obstacles()) {
+ map->remove_obstacle(obstacle);
+ obstacle->set_map(nullptr);
+ }
+
+ int map_index = active_maps.find(map);
+ if (map_index >= 0) {
+ active_maps.remove_at(map_index);
+ active_maps_iteration_id.remove_at(map_index);
+ }
+ map_owner.free(p_object);
+
+ } else if (region_owner.owns(p_object)) {
+ NavRegion2D *region = region_owner.get_or_null(p_object);
+
+ // Removes this region from the map if assigned.
+ if (region->get_map() != nullptr) {
+ region->get_map()->remove_region(region);
+ region->set_map(nullptr);
+ }
+
+ region_owner.free(p_object);
+
+ } else if (link_owner.owns(p_object)) {
+ NavLink2D *link = link_owner.get_or_null(p_object);
+
+ // Removes this link from the map if assigned.
+ if (link->get_map() != nullptr) {
+ link->get_map()->remove_link(link);
+ link->set_map(nullptr);
+ }
+
+ link_owner.free(p_object);
+
+ } else if (agent_owner.owns(p_object)) {
+ internal_free_agent(p_object);
+
+ } else if (obstacle_owner.owns(p_object)) {
+ internal_free_obstacle(p_object);
+
+ } else {
+ ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
+ }
+}
+
+void GodotNavigationServer2D::internal_free_agent(RID p_object) {
+ NavAgent2D *agent = agent_owner.get_or_null(p_object);
+ if (agent) {
+ if (agent->get_map() != nullptr) {
+ agent->get_map()->remove_agent(agent);
+ agent->set_map(nullptr);
+ }
+ agent_owner.free(p_object);
+ }
+}
+
+void GodotNavigationServer2D::internal_free_obstacle(RID p_object) {
+ NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_object);
+ if (obstacle) {
+ NavAgent2D *obstacle_agent = obstacle->get_agent();
+ if (obstacle_agent) {
+ RID _agent_rid = obstacle_agent->get_self();
+ internal_free_agent(_agent_rid);
+ obstacle->set_agent(nullptr);
+ }
+ if (obstacle->get_map() != nullptr) {
+ obstacle->get_map()->remove_obstacle(obstacle);
+ obstacle->set_map(nullptr);
+ }
+ obstacle_owner.free(p_object);
+ }
+}
+
+void GodotNavigationServer2D::process(real_t p_delta_time) {
+ flush_queries();
+
+ if (!active) {
+ return;
+ }
+
+ int _new_pm_region_count = 0;
+ int _new_pm_agent_count = 0;
+ int _new_pm_link_count = 0;
+ int _new_pm_polygon_count = 0;
+ int _new_pm_edge_count = 0;
+ int _new_pm_edge_merge_count = 0;
+ int _new_pm_edge_connection_count = 0;
+ int _new_pm_edge_free_count = 0;
+ int _new_pm_obstacle_count = 0;
+
+ // In c++ we can't be sure that this is performed in the main thread
+ // even with mutable functions.
+ MutexLock lock(operations_mutex);
+ for (uint32_t i(0); i < active_maps.size(); i++) {
+ active_maps[i]->sync();
+ active_maps[i]->step(p_delta_time);
+ active_maps[i]->dispatch_callbacks();
+
+ _new_pm_region_count += active_maps[i]->get_pm_region_count();
+ _new_pm_agent_count += active_maps[i]->get_pm_agent_count();
+ _new_pm_link_count += active_maps[i]->get_pm_link_count();
+ _new_pm_polygon_count += active_maps[i]->get_pm_polygon_count();
+ _new_pm_edge_count += active_maps[i]->get_pm_edge_count();
+ _new_pm_edge_merge_count += active_maps[i]->get_pm_edge_merge_count();
+ _new_pm_edge_connection_count += active_maps[i]->get_pm_edge_connection_count();
+ _new_pm_edge_free_count += active_maps[i]->get_pm_edge_free_count();
+ _new_pm_obstacle_count += active_maps[i]->get_pm_obstacle_count();
+
+ // Emit a signal if a map changed.
+ const uint32_t new_map_iteration_id = active_maps[i]->get_iteration_id();
+ if (new_map_iteration_id != active_maps_iteration_id[i]) {
+ emit_signal(SNAME("map_changed"), active_maps[i]->get_self());
+ active_maps_iteration_id[i] = new_map_iteration_id;
+ }
+ }
+
+ pm_region_count = _new_pm_region_count;
+ pm_agent_count = _new_pm_agent_count;
+ pm_link_count = _new_pm_link_count;
+ pm_polygon_count = _new_pm_polygon_count;
+ pm_edge_count = _new_pm_edge_count;
+ pm_edge_merge_count = _new_pm_edge_merge_count;
+ pm_edge_connection_count = _new_pm_edge_connection_count;
+ pm_edge_free_count = _new_pm_edge_free_count;
+ pm_obstacle_count = _new_pm_obstacle_count;
+}
+
+void GodotNavigationServer2D::set_active(bool p_active) {
+ MutexLock lock(operations_mutex);
+
+ active = p_active;
+}
+
+void GodotNavigationServer2D::query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) {
+ ERR_FAIL_COND(p_query_parameters.is_null());
+ ERR_FAIL_COND(p_query_result.is_null());
+
+ NavMap2D *map = map_owner.get_or_null(p_query_parameters->get_map());
+ ERR_FAIL_NULL(map);
+
+ NavMeshQueries2D::map_query_path(map, p_query_parameters, p_query_result, p_callback);
+}
+
+RID GodotNavigationServer2D::source_geometry_parser_create() {
+ RWLockWrite write_lock(geometry_parser_rwlock);
+
+ RID rid = geometry_parser_owner.make_rid();
+
+ NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
+ parser->self = rid;
+
+ generator_parsers.push_back(parser);
+#ifdef CLIPPER2_ENABLED
+ NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
+#endif
+ return rid;
+}
+
+void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
+ RWLockWrite write_lock(geometry_parser_rwlock);
+
+ NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
+ ERR_FAIL_NULL(parser);
+
+ parser->callback = p_callback;
+}
+
+int GodotNavigationServer2D::get_process_info(ProcessInfo p_info) const {
+ switch (p_info) {
+ case INFO_ACTIVE_MAPS: {
+ return active_maps.size();
+ } break;
+ case INFO_REGION_COUNT: {
+ return pm_region_count;
+ } break;
+ case INFO_AGENT_COUNT: {
+ return pm_agent_count;
+ } break;
+ case INFO_LINK_COUNT: {
+ return pm_link_count;
+ } break;
+ case INFO_POLYGON_COUNT: {
+ return pm_polygon_count;
+ } break;
+ case INFO_EDGE_COUNT: {
+ return pm_edge_count;
+ } break;
+ case INFO_EDGE_MERGE_COUNT: {
+ return pm_edge_merge_count;
+ } break;
+ case INFO_EDGE_CONNECTION_COUNT: {
+ return pm_edge_connection_count;
+ } break;
+ case INFO_EDGE_FREE_COUNT: {
+ return pm_edge_free_count;
+ } break;
+ case INFO_OBSTACLE_COUNT: {
+ return pm_obstacle_count;
+ } break;
+ }
+
+ return 0;
+}
+
+#undef COMMAND_1
+#undef COMMAND_2
diff --git a/modules/navigation/2d/godot_navigation_server_2d.h b/modules/navigation_2d/2d/godot_navigation_server_2d.h
similarity index 65%
rename from modules/navigation/2d/godot_navigation_server_2d.h
rename to modules/navigation_2d/2d/godot_navigation_server_2d.h
index 338f00c607..5188dd5959 100644
--- a/modules/navigation/2d/godot_navigation_server_2d.h
+++ b/modules/navigation_2d/2d/godot_navigation_server_2d.h
@@ -30,75 +30,145 @@
#pragma once
-#include "../nav_agent_3d.h"
-#include "../nav_link_3d.h"
-#include "../nav_map_3d.h"
-#include "../nav_obstacle_3d.h"
-#include "../nav_region_3d.h"
+#include "../nav_agent_2d.h"
+#include "../nav_link_2d.h"
+#include "../nav_map_2d.h"
+#include "../nav_obstacle_2d.h"
+#include "../nav_region_2d.h"
+#include "core/templates/local_vector.h"
+#include "core/templates/rid.h"
+#include "core/templates/rid_owner.h"
+#include "servers/navigation/navigation_path_query_parameters_2d.h"
+#include "servers/navigation/navigation_path_query_result_2d.h"
#include "servers/navigation_server_2d.h"
+/// The commands are functions executed during the `sync` phase.
+
+#define MERGE_INTERNAL(A, B) A##B
+#define MERGE(A, B) MERGE_INTERNAL(A, B)
+
+#define COMMAND_1(F_NAME, T_0, D_0) \
+ virtual void F_NAME(T_0 D_0) override; \
+ void MERGE(_cmd_, F_NAME)(T_0 D_0)
+
+#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1) \
+ virtual void F_NAME(T_0 D_0, T_1 D_1) override; \
+ void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+class GodotNavigationServer2D;
#ifdef CLIPPER2_ENABLED
class NavMeshGenerator2D;
#endif // CLIPPER2_ENABLED
+struct SetCommand2D {
+ virtual ~SetCommand2D() {}
+ virtual void exec(GodotNavigationServer2D *p_server) = 0;
+};
+
// This server exposes the `NavigationServer3D` features in the 2D world.
class GodotNavigationServer2D : public NavigationServer2D {
GDCLASS(GodotNavigationServer2D, NavigationServer2D);
+ Mutex commands_mutex;
+ /// Mutex used to make any operation threadsafe.
+ Mutex operations_mutex;
+
+ LocalVector commands;
+
+ mutable RID_Owner link_owner;
+ mutable RID_Owner map_owner;
+ mutable RID_Owner region_owner;
+ mutable RID_Owner agent_owner;
+ mutable RID_Owner obstacle_owner;
+
+ bool active = true;
+ LocalVector active_maps;
+ LocalVector active_maps_iteration_id;
+
#ifdef CLIPPER2_ENABLED
NavMeshGenerator2D *navmesh_generator_2d = nullptr;
#endif // CLIPPER2_ENABLED
+ // Performance Monitor.
+ int pm_region_count = 0;
+ int pm_agent_count = 0;
+ int pm_link_count = 0;
+ int pm_polygon_count = 0;
+ int pm_edge_count = 0;
+ int pm_edge_merge_count = 0;
+ int pm_edge_connection_count = 0;
+ int pm_edge_free_count = 0;
+ int pm_obstacle_count = 0;
+
public:
GodotNavigationServer2D();
virtual ~GodotNavigationServer2D();
+ void add_command(SetCommand2D *p_command);
+
virtual TypedArray get_maps() const override;
virtual RID map_create() override;
- virtual void map_set_active(RID p_map, bool p_active) override;
+ COMMAND_2(map_set_active, RID, p_map, bool, p_active);
virtual bool map_is_active(RID p_map) const override;
- virtual void map_set_cell_size(RID p_map, real_t p_cell_size) override;
+
+ COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size);
virtual real_t map_get_cell_size(RID p_map) const override;
- virtual void map_set_use_edge_connections(RID p_map, bool p_enabled) override;
+
+ COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled);
virtual bool map_get_use_edge_connections(RID p_map) const override;
- virtual void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) override;
+
+ COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin);
virtual real_t map_get_edge_connection_margin(RID p_map) const override;
- virtual void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override;
+
+ COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
virtual real_t map_get_link_connection_radius(RID p_map) const override;
+
virtual Vector map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
+
virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override;
+
virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override;
+
virtual TypedArray map_get_links(RID p_map) const override;
virtual TypedArray map_get_regions(RID p_map) const override;
virtual TypedArray map_get_agents(RID p_map) const override;
virtual TypedArray map_get_obstacles(RID p_map) const override;
+
virtual void map_force_update(RID p_map) override;
- virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
virtual uint32_t map_get_iteration_id(RID p_map) const override;
- virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) override;
+
+ COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled);
virtual bool map_get_use_async_iterations(RID p_map) const override;
+ virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
+
virtual RID region_create() override;
- virtual void region_set_enabled(RID p_region, bool p_enabled) override;
+
+ COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled);
virtual bool region_get_enabled(RID p_region) const override;
- virtual void region_set_use_edge_connections(RID p_region, bool p_enabled) override;
+
+ COMMAND_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled);
virtual bool region_get_use_edge_connections(RID p_region) const override;
- virtual void region_set_enter_cost(RID p_region, real_t p_enter_cost) override;
+
+ COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost);
virtual real_t region_get_enter_cost(RID p_region) const override;
- virtual void region_set_travel_cost(RID p_region, real_t p_travel_cost) override;
+ COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost);
virtual real_t region_get_travel_cost(RID p_region) const override;
- virtual void region_set_owner_id(RID p_region, ObjectID p_owner_id) override;
+
+ COMMAND_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id);
virtual ObjectID region_get_owner_id(RID p_region) const override;
+
virtual bool region_owns_point(RID p_region, const Vector2 &p_point) const override;
- virtual void region_set_map(RID p_region, RID p_map) override;
+
+ COMMAND_2(region_set_map, RID, p_region, RID, p_map);
virtual RID region_get_map(RID p_region) const override;
- virtual void region_set_navigation_layers(RID p_region, uint32_t p_navigation_layers) override;
+ COMMAND_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers);
virtual uint32_t region_get_navigation_layers(RID p_region) const override;
- virtual void region_set_transform(RID p_region, Transform2D p_transform) override;
+ COMMAND_2(region_set_transform, RID, p_region, Transform2D, p_transform);
virtual Transform2D region_get_transform(RID p_region) const override;
- virtual void region_set_navigation_polygon(RID p_region, Ref p_navigation_polygon) override;
+ COMMAND_2(region_set_navigation_polygon, RID, p_region, Ref, p_navigation_polygon);
virtual int region_get_connections_count(RID p_region) const override;
virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
@@ -109,51 +179,50 @@ public:
virtual RID link_create() override;
/// Set the map of this link.
- virtual void link_set_map(RID p_link, RID p_map) override;
+ COMMAND_2(link_set_map, RID, p_link, RID, p_map);
virtual RID link_get_map(RID p_link) const override;
-
- virtual void link_set_enabled(RID p_link, bool p_enabled) override;
+ COMMAND_2(link_set_enabled, RID, p_link, bool, p_enabled);
virtual bool link_get_enabled(RID p_link) const override;
/// Set whether this link travels in both directions.
- virtual void link_set_bidirectional(RID p_link, bool p_bidirectional) override;
+ COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional);
virtual bool link_is_bidirectional(RID p_link) const override;
/// Set the link's layers.
- virtual void link_set_navigation_layers(RID p_link, uint32_t p_navigation_layers) override;
+ COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers);
virtual uint32_t link_get_navigation_layers(RID p_link) const override;
/// Set the start position of the link.
- virtual void link_set_start_position(RID p_link, Vector2 p_position) override;
+ COMMAND_2(link_set_start_position, RID, p_link, Vector2, p_position);
virtual Vector2 link_get_start_position(RID p_link) const override;
/// Set the end position of the link.
- virtual void link_set_end_position(RID p_link, Vector2 p_position) override;
+ COMMAND_2(link_set_end_position, RID, p_link, Vector2, p_position);
virtual Vector2 link_get_end_position(RID p_link) const override;
/// Set the enter cost of the link.
- virtual void link_set_enter_cost(RID p_link, real_t p_enter_cost) override;
+ COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost);
virtual real_t link_get_enter_cost(RID p_link) const override;
/// Set the travel cost of the link.
- virtual void link_set_travel_cost(RID p_link, real_t p_travel_cost) override;
+ COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost);
virtual real_t link_get_travel_cost(RID p_link) const override;
/// Set the node which manages this link.
- virtual void link_set_owner_id(RID p_link, ObjectID p_owner_id) override;
+ COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id);
virtual ObjectID link_get_owner_id(RID p_link) const override;
/// Creates the agent.
virtual RID agent_create() override;
/// Put the agent in the map.
- virtual void agent_set_map(RID p_agent, RID p_map) override;
+ COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
virtual RID agent_get_map(RID p_agent) const override;
- virtual void agent_set_paused(RID p_agent, bool p_paused) override;
+ COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused);
virtual bool agent_get_paused(RID p_agent) const override;
- virtual void agent_set_avoidance_enabled(RID p_agent, bool p_enabled) override;
+ COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled);
virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
/// The maximum distance (center point to
@@ -163,7 +232,7 @@ public:
/// time of the simulation. If the number is too
/// low, the simulation will not be safe.
/// Must be non-negative.
- virtual void agent_set_neighbor_distance(RID p_agent, real_t p_distance) override;
+ COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance);
virtual real_t agent_get_neighbor_distance(RID p_agent) const override;
/// The maximum number of other agents this
@@ -172,7 +241,7 @@ public:
/// running time of the simulation. If the
/// number is too low, the simulation will not
/// be safe.
- virtual void agent_set_max_neighbors(RID p_agent, int p_count) override;
+ COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
virtual int agent_get_max_neighbors(RID p_agent) const override;
/// The minimal amount of time for which this
@@ -183,73 +252,80 @@ public:
/// other agents, but the less freedom this
/// agent has in choosing its velocities.
/// Must be positive.
- virtual void agent_set_time_horizon_agents(RID p_agent, real_t p_time_horizon) override;
+ COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon);
virtual real_t agent_get_time_horizon_agents(RID p_agent) const override;
- virtual void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon) override;
+ COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon);
virtual real_t agent_get_time_horizon_obstacles(RID p_agent) const override;
/// The radius of this agent.
/// Must be non-negative.
- virtual void agent_set_radius(RID p_agent, real_t p_radius) override;
+ COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
virtual real_t agent_get_radius(RID p_agent) const override;
/// The maximum speed of this agent.
/// Must be non-negative.
- virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed) override;
+ COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
virtual real_t agent_get_max_speed(RID p_agent) const override;
/// forces and agent velocity change in the avoidance simulation, adds simulation instability if done recklessly
- virtual void agent_set_velocity_forced(RID p_agent, Vector2 p_velocity) override;
+ COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity);
/// The wanted velocity for the agent as a "suggestion" to the avoidance simulation.
/// The simulation will try to fulfill this velocity wish if possible but may change the velocity depending on other agent's and obstacles'.
- virtual void agent_set_velocity(RID p_agent, Vector2 p_velocity) override;
+ COMMAND_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity);
virtual Vector2 agent_get_velocity(RID p_agent) const override;
/// Position of the agent in world space.
- virtual void agent_set_position(RID p_agent, Vector2 p_position) override;
+ COMMAND_2(agent_set_position, RID, p_agent, Vector2, p_position);
virtual Vector2 agent_get_position(RID p_agent) const override;
/// Returns true if the map got changed the previous frame.
virtual bool agent_is_map_changed(RID p_agent) const override;
/// Callback called at the end of the RVO process
- virtual void agent_set_avoidance_callback(RID p_agent, Callable p_callback) override;
+ COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback);
virtual bool agent_has_avoidance_callback(RID p_agent) const override;
- virtual void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers) override;
+ COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers);
virtual uint32_t agent_get_avoidance_layers(RID p_agent) const override;
- virtual void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask) override;
+ COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask);
virtual uint32_t agent_get_avoidance_mask(RID p_agent) const override;
- virtual void agent_set_avoidance_priority(RID p_agent, real_t p_priority) override;
+ COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority);
virtual real_t agent_get_avoidance_priority(RID p_agent) const override;
virtual RID obstacle_create() override;
- virtual void obstacle_set_avoidance_enabled(RID p_obstacle, bool p_enabled) override;
+ COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled);
virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
- virtual void obstacle_set_map(RID p_obstacle, RID p_map) override;
+ COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map);
virtual RID obstacle_get_map(RID p_obstacle) const override;
- virtual void obstacle_set_paused(RID p_obstacle, bool p_paused) override;
+ COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused);
virtual bool obstacle_get_paused(RID p_obstacle) const override;
- virtual void obstacle_set_radius(RID p_obstacle, real_t p_radius) override;
+ COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius);
virtual real_t obstacle_get_radius(RID p_obstacle) const override;
- virtual void obstacle_set_velocity(RID p_obstacle, Vector2 p_velocity) override;
+ COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity);
virtual Vector2 obstacle_get_velocity(RID p_obstacle) const override;
- virtual void obstacle_set_position(RID p_obstacle, Vector2 p_position) override;
+ COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position);
virtual Vector2 obstacle_get_position(RID p_obstacle) const override;
- virtual void obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) override;
+ COMMAND_2(obstacle_set_vertices, RID, p_obstacle, const Vector &, p_vertices);
virtual Vector obstacle_get_vertices(RID p_obstacle) const override;
- virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override;
+ COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers);
virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
- virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) override;
+ virtual void query_path(const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback = Callable()) override;
+ COMMAND_1(free, RID, p_object);
+
+ virtual void set_active(bool p_active) override;
+
+ void flush_queries();
+ virtual void process(real_t p_delta_time) override;
virtual void init() override;
virtual void sync() override;
virtual void finish() override;
- virtual void free(RID p_object) override;
+
+ virtual int get_process_info(ProcessInfo p_info) const override;
virtual void parse_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override;
virtual void bake_from_source_geometry_data(const Ref &p_navigation_mesh, const Ref &p_source_geometry_data, const Callable &p_callback = Callable()) override;
@@ -260,4 +336,11 @@ public:
virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) override;
virtual Vector simplify_path(const Vector &p_path, real_t p_epsilon) override;
+
+private:
+ void internal_free_agent(RID p_object);
+ void internal_free_obstacle(RID p_object);
};
+
+#undef COMMAND_1
+#undef COMMAND_2
diff --git a/modules/navigation_2d/2d/nav_base_iteration_2d.h b/modules/navigation_2d/2d/nav_base_iteration_2d.h
new file mode 100644
index 0000000000..feb7c68f67
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_base_iteration_2d.h
@@ -0,0 +1,58 @@
+/**************************************************************************/
+/* nav_base_iteration_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+
+#include "servers/navigation/navigation_utilities.h"
+
+struct NavBaseIteration2D {
+ uint32_t id = UINT32_MAX;
+ bool enabled = true;
+ uint32_t navigation_layers = 1;
+ real_t enter_cost = 0.0;
+ real_t travel_cost = 1.0;
+ NavigationUtilities::PathSegmentType owner_type;
+ ObjectID owner_object_id;
+ RID owner_rid;
+ bool owner_use_edge_connections = false;
+ LocalVector navmesh_polygons;
+
+ bool get_enabled() const { return enabled; }
+ NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
+ RID get_self() const { return owner_rid; }
+ ObjectID get_owner_id() const { return owner_object_id; }
+ uint32_t get_navigation_layers() const { return navigation_layers; }
+ real_t get_enter_cost() const { return enter_cost; }
+ real_t get_travel_cost() const { return travel_cost; }
+ bool get_use_edge_connections() const { return owner_use_edge_connections; }
+ const LocalVector &get_navmesh_polygons() const { return navmesh_polygons; }
+};
diff --git a/modules/navigation_2d/2d/nav_map_builder_2d.cpp b/modules/navigation_2d/2d/nav_map_builder_2d.cpp
new file mode 100644
index 0000000000..c3e8890bcf
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_map_builder_2d.cpp
@@ -0,0 +1,405 @@
+/**************************************************************************/
+/* nav_map_builder_2d.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#include "nav_map_builder_2d.h"
+
+#include "../nav_link_2d.h"
+#include "../nav_map_2d.h"
+#include "../nav_region_2d.h"
+#include "../triangle2.h"
+#include "nav_map_iteration_2d.h"
+#include "nav_region_iteration_2d.h"
+
+using namespace nav_2d;
+
+PointKey NavMapBuilder2D::get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size) {
+ const int x = static_cast(Math::floor(p_pos.x / p_cell_size.x));
+ const int y = static_cast(Math::floor(p_pos.y / p_cell_size.y));
+
+ PointKey p;
+ p.key = 0;
+ p.x = x;
+ p.y = y;
+ return p;
+}
+
+void NavMapBuilder2D::build_navmap_iteration(NavMapIterationBuild2D &r_build) {
+ PerformanceData &performance_data = r_build.performance_data;
+
+ performance_data.pm_polygon_count = 0;
+ performance_data.pm_edge_count = 0;
+ performance_data.pm_edge_merge_count = 0;
+ performance_data.pm_edge_connection_count = 0;
+ performance_data.pm_edge_free_count = 0;
+
+ _build_step_gather_region_polygons(r_build);
+
+ _build_step_find_edge_connection_pairs(r_build);
+
+ _build_step_merge_edge_connection_pairs(r_build);
+
+ _build_step_edge_connection_margin_connections(r_build);
+
+ _build_step_navlink_connections(r_build);
+
+ _build_update_map_iteration(r_build);
+}
+
+void NavMapBuilder2D::_build_step_gather_region_polygons(NavMapIterationBuild2D &r_build) {
+ PerformanceData &performance_data = r_build.performance_data;
+ NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+ LocalVector ®ions = map_iteration->region_iterations;
+ HashMap> ®ion_external_connections = map_iteration->external_region_connections;
+
+ // Remove regions connections.
+ region_external_connections.clear();
+ for (const NavRegionIteration2D ®ion : regions) {
+ region_external_connections[region.id] = LocalVector();
+ }
+
+ // Copy all region polygons in the map.
+ int polygon_count = 0;
+ for (NavRegionIteration2D ®ion : regions) {
+ if (!region.get_enabled()) {
+ continue;
+ }
+ LocalVector &polygons_source = region.navmesh_polygons;
+ for (uint32_t n = 0; n < polygons_source.size(); n++) {
+ polygons_source[n].id = polygon_count;
+ polygon_count++;
+ }
+ }
+
+ performance_data.pm_polygon_count = polygon_count;
+ r_build.polygon_count = polygon_count;
+}
+
+void NavMapBuilder2D::_build_step_find_edge_connection_pairs(NavMapIterationBuild2D &r_build) {
+ PerformanceData &performance_data = r_build.performance_data;
+ NavMapIteration2D *map_iteration = r_build.map_iteration;
+ int polygon_count = r_build.polygon_count;
+
+ HashMap &connection_pairs_map = r_build.iter_connection_pairs_map;
+
+ // Group all edges per key.
+ connection_pairs_map.clear();
+ connection_pairs_map.reserve(polygon_count);
+ int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
+
+ for (NavRegionIteration2D ®ion : map_iteration->region_iterations) {
+ if (!region.get_enabled()) {
+ continue;
+ }
+
+ for (Polygon &poly : region.navmesh_polygons) {
+ for (uint32_t p = 0; p < poly.points.size(); p++) {
+ const int next_point = (p + 1) % poly.points.size();
+ const EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
+
+ HashMap::Iterator pair_it = connection_pairs_map.find(ek);
+ if (!pair_it) {
+ pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
+ performance_data.pm_edge_count += 1;
+ ++free_edges_count;
+ }
+ EdgeConnectionPair &pair = pair_it->value;
+ if (pair.size < 2) {
+ // Add the polygon/edge tuple to this key.
+ Edge::Connection new_connection;
+ new_connection.polygon = &poly;
+ new_connection.edge = p;
+ new_connection.pathway_start = poly.points[p].pos;
+ new_connection.pathway_end = poly.points[next_point].pos;
+
+ pair.connections[pair.size] = new_connection;
+ ++pair.size;
+ if (pair.size == 2) {
+ --free_edges_count;
+ }
+
+ } else {
+ // The edge is already connected with another edge, skip.
+ ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
+ }
+ }
+ }
+ }
+
+ r_build.free_edge_count = free_edges_count;
+}
+
+void NavMapBuilder2D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild2D &r_build) {
+ PerformanceData &performance_data = r_build.performance_data;
+
+ HashMap &connection_pairs_map = r_build.iter_connection_pairs_map;
+ LocalVector &free_edges = r_build.iter_free_edges;
+ int free_edges_count = r_build.free_edge_count;
+ bool use_edge_connections = r_build.use_edge_connections;
+
+ free_edges.clear();
+ free_edges.reserve(free_edges_count);
+
+ for (const KeyValue &pair_it : connection_pairs_map) {
+ const EdgeConnectionPair &pair = pair_it.value;
+ if (pair.size == 2) {
+ // Connect edge that are shared in different polygons.
+ const Edge::Connection &c1 = pair.connections[0];
+ const Edge::Connection &c2 = pair.connections[1];
+ c1.polygon->edges[c1.edge].connections.push_back(c2);
+ c2.polygon->edges[c2.edge].connections.push_back(c1);
+ // Note: The pathway_start/end are full for those connection and do not need to be modified.
+ performance_data.pm_edge_merge_count += 1;
+ } else {
+ CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
+ if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
+ free_edges.push_back(pair.connections[0]);
+ }
+ }
+ }
+}
+
+void NavMapBuilder2D::_build_step_edge_connection_margin_connections(NavMapIterationBuild2D &r_build) {
+ PerformanceData &performance_data = r_build.performance_data;
+ NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+ real_t edge_connection_margin = r_build.edge_connection_margin;
+ LocalVector &free_edges = r_build.iter_free_edges;
+ HashMap> ®ion_external_connections = map_iteration->external_region_connections;
+
+ // Find the compatible near edges.
+ //
+ // Note:
+ // Considering that the edges must be compatible (for obvious reasons)
+ // to be connected, create new polygons to remove that small gap is
+ // not really useful and would result in wasteful computation during
+ // connection, integration and path finding.
+ performance_data.pm_edge_free_count = free_edges.size();
+
+ const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
+
+ for (uint32_t i = 0; i < free_edges.size(); i++) {
+ const Edge::Connection &free_edge = free_edges[i];
+ Vector2 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
+ Vector2 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
+
+ for (uint32_t j = 0; j < free_edges.size(); j++) {
+ const Edge::Connection &other_edge = free_edges[j];
+ if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
+ continue;
+ }
+
+ Vector2 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
+ Vector2 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
+
+ // Compute the projection of the opposite edge on the current one.
+ Vector2 edge_vector = edge_p2 - edge_p1;
+ real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / edge_vector.length_squared();
+ real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / edge_vector.length_squared();
+ if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
+ continue;
+ }
+
+ // Check if the two edges are close to each other enough and compute a pathway between the two regions.
+ Vector2 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+ Vector2 other1;
+ if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
+ other1 = other_edge_p1;
+ } else {
+ other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+ }
+ if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
+ continue;
+ }
+
+ Vector2 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+ Vector2 other2;
+ if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
+ other2 = other_edge_p2;
+ } else {
+ other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+ }
+ if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
+ continue;
+ }
+
+ // The edges can now be connected.
+ Edge::Connection new_connection = other_edge;
+ new_connection.pathway_start = (self1 + other1) / 2.0;
+ new_connection.pathway_end = (self2 + other2) / 2.0;
+ free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
+
+ // Add the connection to the region_connection map.
+ region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
+ performance_data.pm_edge_connection_count += 1;
+ }
+ }
+}
+
+void NavMapBuilder2D::_build_step_navlink_connections(NavMapIterationBuild2D &r_build) {
+ NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+ real_t link_connection_radius = r_build.link_connection_radius;
+ Vector2 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
+
+ LocalVector &link_polygons = map_iteration->link_polygons;
+ LocalVector &links = map_iteration->link_iterations;
+ int polygon_count = r_build.polygon_count;
+
+ real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
+ uint32_t link_poly_idx = 0;
+ link_polygons.resize(links.size());
+
+ // Search for polygons within range of a nav link.
+ for (const NavLinkIteration2D &link : links) {
+ if (!link.get_enabled()) {
+ continue;
+ }
+ const Vector2 link_start_pos = link.get_start_position();
+ const Vector2 link_end_pos = link.get_end_position();
+
+ Polygon *closest_start_polygon = nullptr;
+ real_t closest_start_sqr_dist = link_connection_radius_sqr;
+ Vector2 closest_start_point;
+
+ Polygon *closest_end_polygon = nullptr;
+ real_t closest_end_sqr_dist = link_connection_radius_sqr;
+ Vector2 closest_end_point;
+
+ for (NavRegionIteration2D ®ion : map_iteration->region_iterations) {
+ if (!region.get_enabled()) {
+ continue;
+ }
+ Rect2 region_bounds = region.get_bounds().grow(link_connection_radius);
+ if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
+ continue;
+ }
+
+ for (Polygon &polyon : region.navmesh_polygons) {
+ for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) {
+ const Triangle2 triangle(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos);
+
+ {
+ const Vector2 start_point = triangle.get_closest_point_to(link_start_pos);
+ const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
+
+ // Pick the polygon that is within our radius and is closer than anything we've seen yet.
+ if (sqr_dist < closest_start_sqr_dist) {
+ closest_start_sqr_dist = sqr_dist;
+ closest_start_point = start_point;
+ closest_start_polygon = &polyon;
+ }
+ }
+
+ {
+ const Vector2 end_point = triangle.get_closest_point_to(link_end_pos);
+ const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
+
+ // Pick the polygon that is within our radius and is closer than anything we've seen yet.
+ if (sqr_dist < closest_end_sqr_dist) {
+ closest_end_sqr_dist = sqr_dist;
+ closest_end_point = end_point;
+ closest_end_polygon = &polyon;
+ }
+ }
+ }
+ }
+ }
+
+ // If we have both a start and end point, then create a synthetic polygon to route through.
+ if (closest_start_polygon && closest_end_polygon) {
+ Polygon &new_polygon = link_polygons[link_poly_idx++];
+ new_polygon.id = polygon_count++;
+ new_polygon.owner = &link;
+
+ new_polygon.edges.clear();
+ new_polygon.edges.resize(4);
+ new_polygon.points.resize(4);
+
+ // Build a set of vertices that create a thin polygon going from the start to the end point.
+ new_polygon.points[0] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
+ new_polygon.points[1] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
+ new_polygon.points[2] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
+ new_polygon.points[3] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
+
+ // Setup connections to go forward in the link.
+ {
+ Edge::Connection entry_connection;
+ entry_connection.polygon = &new_polygon;
+ entry_connection.edge = -1;
+ entry_connection.pathway_start = new_polygon.points[0].pos;
+ entry_connection.pathway_end = new_polygon.points[1].pos;
+ closest_start_polygon->edges[0].connections.push_back(entry_connection);
+
+ Edge::Connection exit_connection;
+ exit_connection.polygon = closest_end_polygon;
+ exit_connection.edge = -1;
+ exit_connection.pathway_start = new_polygon.points[2].pos;
+ exit_connection.pathway_end = new_polygon.points[3].pos;
+ new_polygon.edges[2].connections.push_back(exit_connection);
+ }
+
+ // If the link is bi-directional, create connections from the end to the start.
+ if (link.is_bidirectional()) {
+ Edge::Connection entry_connection;
+ entry_connection.polygon = &new_polygon;
+ entry_connection.edge = -1;
+ entry_connection.pathway_start = new_polygon.points[2].pos;
+ entry_connection.pathway_end = new_polygon.points[3].pos;
+ closest_end_polygon->edges[0].connections.push_back(entry_connection);
+
+ Edge::Connection exit_connection;
+ exit_connection.polygon = closest_start_polygon;
+ exit_connection.edge = -1;
+ exit_connection.pathway_start = new_polygon.points[0].pos;
+ exit_connection.pathway_end = new_polygon.points[1].pos;
+ new_polygon.edges[0].connections.push_back(exit_connection);
+ }
+ }
+ }
+}
+
+void NavMapBuilder2D::_build_update_map_iteration(NavMapIterationBuild2D &r_build) {
+ NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+ LocalVector &link_polygons = map_iteration->link_polygons;
+
+ map_iteration->navmesh_polygon_count = r_build.polygon_count;
+ map_iteration->link_polygon_count = link_polygons.size();
+
+ map_iteration->path_query_slots_mutex.lock();
+ for (NavMeshQueries2D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
+ p_path_query_slot.traversable_polys.clear();
+ p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25);
+ p_path_query_slot.path_corridor.clear();
+ p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
+ }
+ map_iteration->path_query_slots_mutex.unlock();
+}
diff --git a/modules/navigation_2d/2d/nav_map_builder_2d.h b/modules/navigation_2d/2d/nav_map_builder_2d.h
new file mode 100644
index 0000000000..1a387e50a8
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_map_builder_2d.h
@@ -0,0 +1,49 @@
+/**************************************************************************/
+/* nav_map_builder_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+
+struct NavMapIterationBuild2D;
+
+class NavMapBuilder2D {
+ static void _build_step_gather_region_polygons(NavMapIterationBuild2D &r_build);
+ static void _build_step_find_edge_connection_pairs(NavMapIterationBuild2D &r_build);
+ static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild2D &r_build);
+ static void _build_step_edge_connection_margin_connections(NavMapIterationBuild2D &r_build);
+ static void _build_step_navlink_connections(NavMapIterationBuild2D &r_build);
+ static void _build_update_map_iteration(NavMapIterationBuild2D &r_build);
+
+public:
+ static nav_2d::PointKey get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size);
+
+ static void build_navmap_iteration(NavMapIterationBuild2D &r_build);
+};
diff --git a/modules/navigation_2d/2d/nav_map_iteration_2d.h b/modules/navigation_2d/2d/nav_map_iteration_2d.h
new file mode 100644
index 0000000000..50ee5a470f
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_map_iteration_2d.h
@@ -0,0 +1,110 @@
+/**************************************************************************/
+/* nav_map_iteration_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_rid_2d.h"
+#include "../nav_utils_2d.h"
+#include "nav_mesh_queries_2d.h"
+
+#include "core/math/math_defs.h"
+#include "core/os/semaphore.h"
+
+struct NavLinkIteration2D;
+class NavRegion2D;
+struct NavRegionIteration2D;
+struct NavMapIteration2D;
+
+struct NavMapIterationBuild2D {
+ Vector2 merge_rasterizer_cell_size;
+ bool use_edge_connections = true;
+ real_t edge_connection_margin;
+ real_t link_connection_radius;
+ nav_2d::PerformanceData performance_data;
+ int polygon_count = 0;
+ int free_edge_count = 0;
+
+ HashMap iter_connection_pairs_map;
+ LocalVector iter_free_edges;
+
+ NavMapIteration2D *map_iteration = nullptr;
+
+ int navmesh_polygon_count = 0;
+ int link_polygon_count = 0;
+
+ void reset() {
+ performance_data.reset();
+
+ iter_connection_pairs_map.clear();
+ iter_free_edges.clear();
+ polygon_count = 0;
+ free_edge_count = 0;
+
+ navmesh_polygon_count = 0;
+ link_polygon_count = 0;
+ }
+};
+
+struct NavMapIteration2D {
+ mutable SafeNumeric users;
+ RWLock rwlock;
+
+ LocalVector link_polygons;
+
+ LocalVector region_iterations;
+ LocalVector link_iterations;
+
+ int navmesh_polygon_count = 0;
+ int link_polygon_count = 0;
+
+ // The edge connections that the map builds on top with the edge connection margin.
+ HashMap> external_region_connections;
+
+ HashMap region_ptr_to_region_id;
+
+ LocalVector path_query_slots;
+ Mutex path_query_slots_mutex;
+ Semaphore path_query_slots_semaphore;
+};
+
+class NavMapIterationRead2D {
+ const NavMapIteration2D &map_iteration;
+
+public:
+ _ALWAYS_INLINE_ NavMapIterationRead2D(const NavMapIteration2D &p_iteration) :
+ map_iteration(p_iteration) {
+ map_iteration.rwlock.read_lock();
+ map_iteration.users.increment();
+ }
+ _ALWAYS_INLINE_ ~NavMapIterationRead2D() {
+ map_iteration.users.decrement();
+ map_iteration.rwlock.read_unlock();
+ }
+};
diff --git a/modules/navigation/2d/nav_mesh_generator_2d.cpp b/modules/navigation_2d/2d/nav_mesh_generator_2d.cpp
similarity index 100%
rename from modules/navigation/2d/nav_mesh_generator_2d.cpp
rename to modules/navigation_2d/2d/nav_mesh_generator_2d.cpp
diff --git a/modules/navigation/2d/nav_mesh_generator_2d.h b/modules/navigation_2d/2d/nav_mesh_generator_2d.h
similarity index 100%
rename from modules/navigation/2d/nav_mesh_generator_2d.h
rename to modules/navigation_2d/2d/nav_mesh_generator_2d.h
diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp
new file mode 100644
index 0000000000..650061d441
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp
@@ -0,0 +1,1062 @@
+/**************************************************************************/
+/* nav_mesh_queries_2d.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#include "nav_mesh_queries_2d.h"
+
+#include "../nav_base_2d.h"
+#include "../nav_map_2d.h"
+#include "../triangle2.h"
+#include "nav_region_iteration_2d.h"
+
+#include "core/math/geometry_2d.h"
+#include "servers/navigation/navigation_utilities.h"
+
+using namespace nav_2d;
+
+#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
+
+bool NavMeshQueries2D::emit_callback(const Callable &p_callback) {
+ ERR_FAIL_COND_V(!p_callback.is_valid(), false);
+
+ Callable::CallError ce;
+ Variant result;
+ p_callback.callp(nullptr, 0, result, ce);
+
+ return ce.error == Callable::CallError::CALL_OK;
+}
+
+Vector2 NavMeshQueries2D::polygons_get_random_point(const LocalVector &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
+ const LocalVector ®ion_polygons = p_polygons;
+
+ if (region_polygons.is_empty()) {
+ return Vector2();
+ }
+
+ if (p_uniformly) {
+ real_t accumulated_area = 0;
+ RBMap region_area_map;
+
+ for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
+ const Polygon ®ion_polygon = region_polygons[rp_index];
+ real_t polyon_area = region_polygon.surface_area;
+
+ if (polyon_area == 0.0) {
+ continue;
+ }
+ region_area_map[accumulated_area] = rp_index;
+ accumulated_area += polyon_area;
+ }
+ if (region_area_map.is_empty() || accumulated_area == 0) {
+ // All polygons have no real surface / no area.
+ return Vector2();
+ }
+
+ real_t region_area_map_pos = Math::random(real_t(0), accumulated_area);
+
+ RBMap::Iterator region_E = region_area_map.find_closest(region_area_map_pos);
+ ERR_FAIL_COND_V(!region_E, Vector2());
+ uint32_t rrp_polygon_index = region_E->value;
+ ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector2());
+
+ const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
+
+ real_t accumulated_polygon_area = 0;
+ RBMap polygon_area_map;
+
+ for (uint32_t rpp_index = 2; rpp_index < rr_polygon.points.size(); rpp_index++) {
+ real_t triangle_area = Triangle2(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area();
+
+ if (triangle_area == 0.0) {
+ continue;
+ }
+ polygon_area_map[accumulated_polygon_area] = rpp_index;
+ accumulated_polygon_area += triangle_area;
+ }
+ if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) {
+ // All faces have no real surface / no area.
+ return Vector2();
+ }
+
+ real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area);
+
+ RBMap::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos);
+ ERR_FAIL_COND_V(!polygon_E, Vector2());
+ uint32_t rrp_face_index = polygon_E->value;
+ ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector2());
+
+ const Triangle2 triangle(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
+
+ Vector2 triangle_random_position = triangle.get_random_point_inside();
+ return triangle_random_position;
+
+ } else {
+ uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1);
+
+ const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
+
+ uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1);
+
+ const Triangle2 triangle(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
+
+ Vector2 triangle_random_position = triangle.get_random_point_inside();
+ return triangle_random_position;
+ }
+}
+
+void NavMeshQueries2D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask2D &p_query_task, const Vector2 &p_point, const Polygon *p_point_polygon) {
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+ p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+ p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+ p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
+ }
+
+ p_query_task.path_points.push_back(p_point);
+}
+
+void NavMeshQueries2D::map_query_path(NavMap2D *p_map, const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) {
+ ERR_FAIL_NULL(p_map);
+ ERR_FAIL_COND(p_query_parameters.is_null());
+ ERR_FAIL_COND(p_query_result.is_null());
+
+ using namespace NavigationUtilities;
+
+ NavMeshQueries2D::NavMeshPathQueryTask2D query_task;
+ query_task.start_position = p_query_parameters->get_start_position();
+ query_task.target_position = p_query_parameters->get_target_position();
+ query_task.navigation_layers = p_query_parameters->get_navigation_layers();
+ query_task.callback = p_callback;
+
+ const TypedArray &_excluded_regions = p_query_parameters->get_excluded_regions();
+ const TypedArray &_included_regions = p_query_parameters->get_included_regions();
+
+ uint32_t _excluded_region_count = _excluded_regions.size();
+ uint32_t _included_region_count = _included_regions.size();
+
+ query_task.exclude_regions = _excluded_region_count > 0;
+ query_task.include_regions = _included_region_count > 0;
+
+ if (query_task.exclude_regions) {
+ query_task.excluded_regions.resize(_excluded_region_count);
+ for (uint32_t i = 0; i < _excluded_region_count; i++) {
+ query_task.excluded_regions[i] = _excluded_regions[i];
+ }
+ }
+
+ if (query_task.include_regions) {
+ query_task.included_regions.resize(_included_region_count);
+ for (uint32_t i = 0; i < _included_region_count; i++) {
+ query_task.included_regions[i] = _included_regions[i];
+ }
+ }
+
+ switch (p_query_parameters->get_pathfinding_algorithm()) {
+ case NavigationPathQueryParameters2D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: {
+ query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+ } break;
+ default: {
+ WARN_PRINT("No match for used PathfindingAlgorithm - fallback to default");
+ query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+ } break;
+ }
+
+ switch (p_query_parameters->get_path_postprocessing()) {
+ case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+ query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+ } break;
+ case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+ query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED;
+ } break;
+ case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+ query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_NONE;
+ } break;
+ default: {
+ WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+ query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+ } break;
+ }
+
+ query_task.metadata_flags = (int64_t)p_query_parameters->get_metadata_flags();
+ query_task.simplify_path = p_query_parameters->get_simplify_path();
+ query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon();
+ query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_STARTED;
+
+ p_map->query_path(query_task);
+
+ p_query_result->set_data(
+ query_task.path_points,
+ query_task.path_meta_point_types,
+ query_task.path_meta_point_rids,
+ query_task.path_meta_point_owners);
+
+ if (query_task.callback.is_valid()) {
+ if (emit_callback(query_task.callback)) {
+ query_task.status = NavMeshPathQueryTask2D::TaskStatus::CALLBACK_DISPATCHED;
+ } else {
+ query_task.status = NavMeshPathQueryTask2D::TaskStatus::CALLBACK_FAILED;
+ }
+ }
+}
+
+void NavMeshQueries2D::_query_task_find_start_end_positions(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) {
+ real_t begin_d = FLT_MAX;
+ real_t end_d = FLT_MAX;
+
+ const LocalVector ®ions = p_map_iteration.region_iterations;
+
+ for (const NavRegionIteration2D ®ion : regions) {
+ if (!region.get_enabled()) {
+ continue;
+ }
+
+ if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(region.get_self())) {
+ continue;
+ }
+ if (p_query_task.include_regions && !p_query_task.included_regions.has(region.get_self())) {
+ continue;
+ }
+
+ // Find the initial poly and the end poly on this map.
+ for (const Polygon &p : region.get_navmesh_polygons()) {
+ // Only consider the polygon if it in a region with compatible layers.
+ if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
+ continue;
+ }
+
+ // For each triangle check the distance between the origin/destination.
+ for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+ const Triangle2 triangle(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+
+ Vector2 point = triangle.get_closest_point_to(p_query_task.start_position);
+ real_t distance_to_point = point.distance_to(p_query_task.start_position);
+ if (distance_to_point < begin_d) {
+ begin_d = distance_to_point;
+ p_query_task.begin_polygon = &p;
+ p_query_task.begin_position = point;
+ }
+
+ point = triangle.get_closest_point_to(p_query_task.target_position);
+ distance_to_point = point.distance_to(p_query_task.target_position);
+ if (distance_to_point < end_d) {
+ end_d = distance_to_point;
+ p_query_task.end_polygon = &p;
+ p_query_task.end_position = point;
+ }
+ }
+ }
+ }
+}
+
+void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task) {
+ const Vector2 p_target_position = p_query_task.target_position;
+ const Polygon *begin_poly = p_query_task.begin_polygon;
+ const Polygon *end_poly = p_query_task.end_polygon;
+ Vector2 begin_point = p_query_task.begin_position;
+ Vector2 end_point = p_query_task.end_position;
+
+ // Heap of polygons to travel next.
+ Heap
+ &traversable_polys = p_query_task.path_query_slot->traversable_polys;
+ traversable_polys.clear();
+
+ LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor;
+ for (NavigationPoly &polygon : navigation_polys) {
+ polygon.reset();
+ }
+
+ // Initialize the matching navigation polygon.
+ NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
+ begin_navigation_poly.poly = begin_poly;
+ begin_navigation_poly.entry = begin_point;
+ begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
+ begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
+ begin_navigation_poly.traveled_distance = 0.0;
+
+ // This is an implementation of the A* algorithm.
+ uint32_t least_cost_id = begin_poly->id;
+ bool found_route = false;
+
+ const Polygon *reachable_end = nullptr;
+ real_t distance_to_reachable_end = FLT_MAX;
+ bool is_reachable = true;
+ real_t poly_enter_cost = 0.0;
+
+ while (true) {
+ const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
+ real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
+
+ // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
+ for (const Edge &edge : least_cost_poly.poly->edges) {
+ // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
+ for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
+ const Edge::Connection &connection = edge.connections[connection_index];
+
+ const NavBaseIteration2D *connection_owner = connection.polygon->owner;
+ const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner);
+ if (!owner_is_usable) {
+ continue;
+ }
+
+ Vector2 pathway[2] = { connection.pathway_start, connection.pathway_end };
+ const Vector2 new_entry = Geometry2D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
+ const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance;
+
+ // Check if the neighbor polygon has already been processed.
+ NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
+ if (new_traveled_distance < neighbor_poly.traveled_distance) {
+ // Add the polygon to the heap of polygons to traverse next.
+ neighbor_poly.back_navigation_poly_id = least_cost_id;
+ neighbor_poly.back_navigation_edge = connection.edge;
+ neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
+ neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
+ neighbor_poly.traveled_distance = new_traveled_distance;
+ neighbor_poly.distance_to_destination =
+ new_entry.distance_to(end_point) *
+ connection_owner->get_travel_cost();
+ neighbor_poly.entry = new_entry;
+
+ if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
+ traversable_polys.shift(neighbor_poly.traversable_poly_index);
+ } else {
+ neighbor_poly.poly = connection.polygon;
+ traversable_polys.push(&neighbor_poly);
+ }
+ }
+ }
+ }
+
+ poly_enter_cost = 0;
+ // When the heap of traversable polygons is empty at this point it means the end polygon is
+ // unreachable.
+ if (traversable_polys.is_empty()) {
+ // Thus use the further reachable polygon
+ ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
+ is_reachable = false;
+ if (reachable_end == nullptr) {
+ // The path is not found and there is not a way out.
+ break;
+ }
+
+ // Set as end point the furthest reachable point.
+ end_poly = reachable_end;
+ real_t end_d = FLT_MAX;
+ for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
+ Triangle2 t(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+ Vector2 spoint = t.get_closest_point_to(p_target_position);
+ real_t dpoint = spoint.distance_squared_to(p_target_position);
+ if (dpoint < end_d) {
+ end_point = spoint;
+ end_d = dpoint;
+ }
+ }
+
+ // Search all faces of start polygon as well.
+ bool closest_point_on_start_poly = false;
+ for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
+ Triangle2 t(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
+ Vector2 spoint = t.get_closest_point_to(p_target_position);
+ real_t dpoint = spoint.distance_squared_to(p_target_position);
+ if (dpoint < end_d) {
+ end_point = spoint;
+ end_d = dpoint;
+ closest_point_on_start_poly = true;
+ }
+ }
+
+ if (closest_point_on_start_poly) {
+ // No point to run PostProcessing when start and end convex polygon is the same.
+ p_query_task.path_clear();
+
+ _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+ _query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
+ p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+ return;
+ }
+
+ for (NavigationPoly &nav_poly : navigation_polys) {
+ nav_poly.poly = nullptr;
+ nav_poly.traveled_distance = FLT_MAX;
+ }
+ navigation_polys[begin_poly->id].poly = begin_poly;
+ navigation_polys[begin_poly->id].traveled_distance = 0;
+ least_cost_id = begin_poly->id;
+ reachable_end = nullptr;
+ } else {
+ // Pop the polygon with the lowest travel cost from the heap of traversable polygons.
+ least_cost_id = traversable_polys.pop()->poly->id;
+
+ // Store the farthest reachable end polygon in case our goal is not reachable.
+ if (is_reachable) {
+ real_t distance = navigation_polys[least_cost_id].entry.distance_squared_to(p_target_position);
+ if (distance_to_reachable_end > distance) {
+ distance_to_reachable_end = distance;
+ reachable_end = navigation_polys[least_cost_id].poly;
+ }
+ }
+
+ // Check if we reached the end
+ if (navigation_polys[least_cost_id].poly == end_poly) {
+ found_route = true;
+ break;
+ }
+
+ if (navigation_polys[least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) {
+ poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
+ }
+ }
+ }
+
+ // We did not find a route but we have both a start polygon and an end polygon at this point.
+ // Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon.
+ if (!found_route) {
+ real_t end_d = FLT_MAX;
+ // Search all faces of the start polygon for the closest point to our target position.
+ for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
+ Triangle2 t(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
+ Vector2 spoint = t.get_closest_point_to(p_target_position);
+ real_t dpoint = spoint.distance_squared_to(p_target_position);
+ if (dpoint < end_d) {
+ end_point = spoint;
+ end_d = dpoint;
+ }
+ }
+
+ p_query_task.path_clear();
+
+ _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+ _query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
+ p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+ } else {
+ p_query_task.end_position = end_point;
+ p_query_task.end_polygon = end_poly;
+ p_query_task.begin_position = begin_point;
+ p_query_task.begin_polygon = begin_poly;
+ p_query_task.least_cost_id = least_cost_id;
+ }
+}
+
+void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) {
+ p_query_task.path_clear();
+
+ _query_task_find_start_end_positions(p_query_task, p_map_iteration);
+
+ // Check for trivial cases.
+ if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
+ p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+ return;
+ }
+ if (p_query_task.begin_polygon == p_query_task.end_polygon) {
+ p_query_task.path_clear();
+ _query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon);
+ _query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon);
+ p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+ return;
+ }
+
+ _query_task_build_path_corridor(p_query_task);
+
+ if (p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FAILED) {
+ return;
+ }
+
+ // Post-Process path.
+ switch (p_query_task.path_postprocessing) {
+ case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+ _query_task_post_process_corridorfunnel(p_query_task);
+ } break;
+ case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+ _query_task_post_process_edgecentered(p_query_task);
+ } break;
+ case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+ _query_task_post_process_nopostprocessing(p_query_task);
+ } break;
+ default: {
+ WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+ _query_task_post_process_corridorfunnel(p_query_task);
+ } break;
+ }
+
+ p_query_task.path_reverse();
+
+ if (p_query_task.simplify_path) {
+ _query_task_simplified_path_points(p_query_task);
+ }
+
+#ifdef DEBUG_ENABLED
+ // Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+ DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+ DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+ DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
+ }
+#endif // DEBUG_ENABLED
+
+ p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+}
+
+void NavMeshQueries2D::_query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task) {
+ if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) {
+ return;
+ }
+
+ const LocalVector &simplified_path_indices = NavMeshQueries2D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon);
+
+ uint32_t index_count = simplified_path_indices.size();
+
+ {
+ Vector2 *points_ptr = p_query_task.path_points.ptr();
+ for (uint32_t i = 0; i < index_count; i++) {
+ points_ptr[i] = points_ptr[simplified_path_indices[i]];
+ }
+ p_query_task.path_points.resize(index_count);
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+ int32_t *types_ptr = p_query_task.path_meta_point_types.ptr();
+ for (uint32_t i = 0; i < index_count; i++) {
+ types_ptr[i] = types_ptr[simplified_path_indices[i]];
+ }
+ p_query_task.path_meta_point_types.resize(index_count);
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+ RID *rids_ptr = p_query_task.path_meta_point_rids.ptr();
+ for (uint32_t i = 0; i < index_count; i++) {
+ rids_ptr[i] = rids_ptr[simplified_path_indices[i]];
+ }
+ p_query_task.path_meta_point_rids.resize(index_count);
+ }
+
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+ int64_t *owners_ptr = p_query_task.path_meta_point_owners.ptr();
+ for (uint32_t i = 0; i < index_count; i++) {
+ owners_ptr[i] = owners_ptr[simplified_path_indices[i]];
+ }
+ p_query_task.path_meta_point_owners.resize(index_count);
+ }
+}
+
+void NavMeshQueries2D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask2D &p_query_task) {
+ Vector2 end_point = p_query_task.end_position;
+ const Polygon *end_poly = p_query_task.end_polygon;
+ Vector2 begin_point = p_query_task.begin_position;
+ const Polygon *begin_poly = p_query_task.begin_polygon;
+ uint32_t least_cost_id = p_query_task.least_cost_id;
+ LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+ // Set the apex poly/point to the end point.
+ NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+
+ Vector2 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
+ const Vector2 back_edge_closest_point = Geometry2D::get_closest_point_to_segment(end_point, back_pathway);
+ if (end_point.is_equal_approx(back_edge_closest_point)) {
+ // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
+ // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
+ if (apex_poly->back_navigation_poly_id != -1) {
+ apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
+ }
+ }
+
+ Vector2 apex_point = end_point;
+
+ NavigationPoly *left_poly = apex_poly;
+ Vector2 left_portal = apex_point;
+ NavigationPoly *right_poly = apex_poly;
+ Vector2 right_portal = apex_point;
+
+ NavigationPoly *p = apex_poly;
+
+ _query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
+
+ while (p) {
+ // Set left and right points of the pathway between polygons.
+ Vector2 left = p->back_navigation_edge_pathway_start;
+ Vector2 right = p->back_navigation_edge_pathway_end;
+ if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right) < 0) {
+ SWAP(left, right);
+ }
+
+ bool skip = false;
+ if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left) >= 0) {
+ // Process.
+ if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal) > 0) {
+ left_poly = p;
+ left_portal = left;
+ } else {
+ _query_task_clip_path(p_query_task, apex_poly, right_portal, right_poly);
+
+ apex_point = right_portal;
+ p = right_poly;
+ left_poly = p;
+ apex_poly = p;
+ left_portal = apex_point;
+ right_portal = apex_point;
+
+ _query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
+ skip = true;
+ }
+ }
+
+ if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right) <= 0) {
+ // Process.
+ if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal) < 0) {
+ right_poly = p;
+ right_portal = right;
+ } else {
+ _query_task_clip_path(p_query_task, apex_poly, left_portal, left_poly);
+
+ apex_point = left_portal;
+ p = left_poly;
+ right_poly = p;
+ apex_poly = p;
+ right_portal = apex_point;
+ left_portal = apex_point;
+
+ _query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
+ }
+ }
+
+ // Go to the previous polygon.
+ if (p->back_navigation_poly_id != -1) {
+ p = &navigation_polys[p->back_navigation_poly_id];
+ } else {
+ // The end
+ p = nullptr;
+ }
+ }
+
+ // If the last point is not the begin point, add it to the list.
+ if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_point) {
+ _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+ }
+}
+
+void NavMeshQueries2D::_query_task_post_process_edgecentered(NavMeshPathQueryTask2D &p_query_task) {
+ Vector2 end_point = p_query_task.end_position;
+ const Polygon *end_poly = p_query_task.end_polygon;
+ Vector2 begin_point = p_query_task.begin_position;
+ const Polygon *begin_poly = p_query_task.begin_polygon;
+ uint32_t least_cost_id = p_query_task.least_cost_id;
+ LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+ _query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
+
+ // Add mid points.
+ int np_id = least_cost_id;
+ while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
+ if (navigation_polys[np_id].back_navigation_edge != -1) {
+ int prev = navigation_polys[np_id].back_navigation_edge;
+ int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
+ Vector2 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
+
+ _query_task_push_back_point_with_metadata(p_query_task, point, navigation_polys[np_id].poly);
+ } else {
+ _query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
+ }
+
+ np_id = navigation_polys[np_id].back_navigation_poly_id;
+ }
+
+ _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+}
+
+void NavMeshQueries2D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask2D &p_query_task) {
+ Vector2 end_point = p_query_task.end_position;
+ const Polygon *end_poly = p_query_task.end_polygon;
+ Vector2 begin_point = p_query_task.begin_position;
+ const Polygon *begin_poly = p_query_task.begin_polygon;
+ uint32_t least_cost_id = p_query_task.least_cost_id;
+ LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+ _query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
+
+ // Add mid points.
+ int np_id = least_cost_id;
+ while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
+ _query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
+
+ np_id = navigation_polys[np_id].back_navigation_poly_id;
+ }
+
+ _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+}
+
+Vector2 NavMeshQueries2D::map_iteration_get_closest_point(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) {
+ ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+ return cp.point;
+}
+
+RID NavMeshQueries2D::map_iteration_get_closest_point_owner(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) {
+ ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+ return cp.owner;
+}
+
+ClosestPointQueryResult NavMeshQueries2D::map_iteration_get_closest_point_info(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) {
+ ClosestPointQueryResult result;
+ real_t closest_point_distance_squared = FLT_MAX;
+
+ // TODO: Check for further 2D improvements.
+
+ const LocalVector ®ions = p_map_iteration.region_iterations;
+ for (const NavRegionIteration2D ®ion : regions) {
+ for (const Polygon &polygon : region.get_navmesh_polygons()) {
+ real_t cross = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
+ Vector2 closest_on_polygon;
+ real_t closest = FLT_MAX;
+ bool inside = true;
+ Vector2 previous = polygon.points[polygon.points.size() - 1].pos;
+ for (size_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
+ Vector2 edge = polygon.points[point_id].pos - previous;
+ Vector2 to_point = p_point - previous;
+ real_t edge_to_point_cross = edge.cross(to_point);
+ bool clockwise = (edge_to_point_cross * cross) > 0;
+ // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge.
+ if (!clockwise) {
+ inside = false;
+ real_t point_projected_on_edge = edge.dot(to_point);
+ real_t edge_square = edge.length_squared();
+
+ if (point_projected_on_edge > edge_square) {
+ real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
+ if (distance < closest) {
+ closest_on_polygon = polygon.points[point_id].pos;
+ closest = distance;
+ }
+ } else if (point_projected_on_edge < 0.0) {
+ real_t distance = previous.distance_squared_to(p_point);
+ if (distance < closest) {
+ closest_on_polygon = previous;
+ closest = distance;
+ }
+ } else {
+ // If we project on this edge, this will be the closest point.
+ real_t percent = point_projected_on_edge / edge_square;
+ closest_on_polygon = previous + percent * edge;
+ break;
+ }
+ }
+ previous = polygon.points[point_id].pos;
+ }
+
+ if (inside) {
+ closest_point_distance_squared = 0.0;
+ result.point = p_point;
+ result.owner = polygon.owner->get_self();
+
+ break;
+ } else {
+ real_t distance = closest_on_polygon.distance_squared_to(p_point);
+ if (distance < closest_point_distance_squared) {
+ closest_point_distance_squared = distance;
+ result.point = closest_on_polygon;
+ result.owner = polygon.owner->get_self();
+ }
+ }
+ }
+ }
+
+ return result;
+}
+
+Vector2 NavMeshQueries2D::map_iteration_get_random_point(const NavMapIteration2D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) {
+ if (p_map_iteration.region_iterations.is_empty()) {
+ return Vector2();
+ }
+
+ LocalVector accessible_regions;
+ accessible_regions.reserve(p_map_iteration.region_iterations.size());
+
+ for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) {
+ const NavRegionIteration2D ®ion = p_map_iteration.region_iterations[i];
+ if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
+ continue;
+ }
+ accessible_regions.push_back(i);
+ }
+
+ if (accessible_regions.is_empty()) {
+ // All existing region polygons are disabled.
+ return Vector2();
+ }
+
+ if (p_uniformly) {
+ real_t accumulated_region_surface_area = 0;
+ RBMap accessible_regions_area_map;
+
+ for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) {
+ const NavRegionIteration2D ®ion = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
+
+ real_t region_surface_area = region.surface_area;
+
+ if (region_surface_area == 0.0f) {
+ continue;
+ }
+
+ accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index;
+ accumulated_region_surface_area += region_surface_area;
+ }
+ if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) {
+ // All faces have no real surface / no area.
+ return Vector2();
+ }
+
+ real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area);
+
+ RBMap::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map);
+ ERR_FAIL_COND_V(!E, Vector2());
+ uint32_t random_region_index = E->value;
+ ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector2());
+
+ const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+
+ return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+
+ } else {
+ uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
+
+ const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+
+ return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+ }
+}
+
+Vector2 NavMeshQueries2D::polygons_get_closest_point(const LocalVector &p_polygons, const Vector2 &p_point) {
+ ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
+ return cp.point;
+}
+
+ClosestPointQueryResult NavMeshQueries2D::polygons_get_closest_point_info(const LocalVector &p_polygons, const Vector2 &p_point) {
+ ClosestPointQueryResult result;
+ real_t closest_point_distance_squared = FLT_MAX;
+
+ // TODO: Check for further 2D improvements.
+
+ for (const Polygon &polygon : p_polygons) {
+ real_t cross = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
+ Vector2 closest_on_polygon;
+ real_t closest = FLT_MAX;
+ bool inside = true;
+ Vector2 previous = polygon.points[polygon.points.size() - 1].pos;
+ for (size_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
+ Vector2 edge = polygon.points[point_id].pos - previous;
+ Vector2 to_point = p_point - previous;
+ real_t edge_to_point_cross = edge.cross(to_point);
+ bool clockwise = (edge_to_point_cross * cross) > 0;
+ // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge.
+ if (!clockwise) {
+ inside = false;
+ real_t point_projected_on_edge = edge.dot(to_point);
+ real_t edge_square = edge.length_squared();
+
+ if (point_projected_on_edge > edge_square) {
+ real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
+ if (distance < closest) {
+ closest_on_polygon = polygon.points[point_id].pos;
+ closest = distance;
+ }
+ } else if (point_projected_on_edge < 0.0) {
+ real_t distance = previous.distance_squared_to(p_point);
+ if (distance < closest) {
+ closest_on_polygon = previous;
+ closest = distance;
+ }
+ } else {
+ // If we project on this edge, this will be the closest point.
+ real_t percent = point_projected_on_edge / edge_square;
+ closest_on_polygon = previous + percent * edge;
+ break;
+ }
+ }
+ previous = polygon.points[point_id].pos;
+ }
+
+ if (inside) {
+ closest_point_distance_squared = 0.0;
+ result.point = p_point;
+ result.owner = polygon.owner->get_self();
+ break;
+ } else {
+ real_t distance = closest_on_polygon.distance_squared_to(p_point);
+ if (distance < closest_point_distance_squared) {
+ closest_point_distance_squared = distance;
+ result.point = closest_on_polygon;
+ result.owner = polygon.owner->get_self();
+ }
+ }
+ }
+
+ return result;
+}
+
+RID NavMeshQueries2D::polygons_get_closest_point_owner(const LocalVector &p_polygons, const Vector2 &p_point) {
+ ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
+ return cp.owner;
+}
+
+static bool _line_intersects_segment(const Vector2 &p_line_normal, real_t p_line_d, const Vector2 &p_segment_begin, const Vector2 &p_segment_end, Vector2 &r_intersection) {
+ Vector2 segment = p_segment_begin - p_segment_end;
+ real_t den = p_line_normal.dot(segment);
+
+ if (Math::is_zero_approx(den)) {
+ return false;
+ }
+
+ real_t dist = (p_line_normal.dot(p_segment_begin) - p_line_d) / den;
+
+ if (dist < (real_t)-CMP_EPSILON || dist > (1.0 + (real_t)CMP_EPSILON)) {
+ return false;
+ }
+
+ r_intersection = p_segment_begin - segment * dist;
+ return true;
+}
+
+void NavMeshQueries2D::_query_task_clip_path(NavMeshPathQueryTask2D &p_query_task, const NavigationPoly *p_from_poly, const Vector2 &p_to_point, const NavigationPoly *p_to_poly) {
+ Vector2 from = p_query_task.path_points[p_query_task.path_points.size() - 1];
+ const LocalVector &p_navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+ if (from.is_equal_approx(p_to_point)) {
+ return;
+ }
+
+ // Compute line parameters (equivalent to the Plane case in 3D).
+ const Vector2 normal = -(from - p_to_point).orthogonal().normalized();
+ const real_t d = normal.dot(from);
+
+ while (p_from_poly != p_to_poly) {
+ Vector2 pathway_start = p_from_poly->back_navigation_edge_pathway_start;
+ Vector2 pathway_end = p_from_poly->back_navigation_edge_pathway_end;
+
+ ERR_FAIL_COND(p_from_poly->back_navigation_poly_id == -1);
+ p_from_poly = &p_navigation_polys[p_from_poly->back_navigation_poly_id];
+
+ if (!pathway_start.is_equal_approx(pathway_end)) {
+ Vector2 inters;
+ if (_line_intersects_segment(normal, d, pathway_start, pathway_end, inters)) {
+ if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(p_query_task.path_points[p_query_task.path_points.size() - 1])) {
+ _query_task_push_back_point_with_metadata(p_query_task, inters, p_from_poly->poly);
+ }
+ }
+ }
+ }
+}
+
+bool NavMeshQueries2D::_query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner) {
+ bool owner_usable = true;
+
+ if ((p_query_task.navigation_layers & p_owner->get_navigation_layers()) == 0) {
+ // Not usable. No matching bit between task filter bitmask and owner bitmask.
+ owner_usable = false;
+ return owner_usable;
+ }
+
+ if (p_query_task.exclude_regions || p_query_task.include_regions) {
+ switch (p_owner->get_type()) {
+ case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION: {
+ if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(p_owner->get_self())) {
+ // Not usable. Exclude region filter is active and this region is excluded.
+ owner_usable = false;
+ } else if (p_query_task.include_regions && !p_query_task.included_regions.has(p_owner->get_self())) {
+ // Not usable. Include region filter is active and this region is not included.
+ owner_usable = false;
+ }
+ } break;
+ case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: {
+ const LocalVector &link_polygons = p_owner->get_navmesh_polygons();
+ if (link_polygons.size() != 2) {
+ // Not usable. Whatever this is, it is not a valid connected link.
+ owner_usable = false;
+ } else {
+ const RID link_start_region = link_polygons[0].owner->get_self();
+ const RID link_end_region = link_polygons[1].owner->get_self();
+ if (p_query_task.exclude_regions && (p_query_task.excluded_regions.has(link_start_region) || p_query_task.excluded_regions.has(link_end_region))) {
+ // Not usable. Exclude region filter is active and at least one region of the link is excluded.
+ owner_usable = false;
+ }
+ if (p_query_task.include_regions && (!p_query_task.included_regions.has(link_start_region) || !p_query_task.excluded_regions.has(link_end_region))) {
+ // Not usable. Include region filter is active and not both regions of the links are included.
+ owner_usable = false;
+ }
+ }
+ } break;
+ }
+ }
+
+ return owner_usable;
+}
+
+LocalVector NavMeshQueries2D::get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon) {
+ p_epsilon = MAX(0.0, p_epsilon);
+ real_t squared_epsilon = p_epsilon * p_epsilon;
+
+ LocalVector simplified_path_indices;
+ simplified_path_indices.reserve(p_path.size());
+ simplified_path_indices.push_back(0);
+ simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, simplified_path_indices);
+ simplified_path_indices.push_back(p_path.size() - 1);
+
+ return simplified_path_indices;
+}
+
+void NavMeshQueries2D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_simplified_path_indices) {
+ Vector2 path_segment[2] = { p_points[p_start_inx], p_points[p_end_inx] };
+
+ real_t point_max_distance = 0.0;
+ int point_max_index = 0;
+
+ for (int i = p_start_inx; i < p_end_inx; i++) {
+ const Vector2 &checked_point = p_points[i];
+
+ const Vector2 closest_point = Geometry2D::get_closest_point_to_segment(checked_point, path_segment);
+ real_t distance_squared = closest_point.distance_squared_to(checked_point);
+
+ if (distance_squared > point_max_distance) {
+ point_max_index = i;
+ point_max_distance = distance_squared;
+ }
+ }
+
+ if (point_max_distance > p_epsilon) {
+ simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_simplified_path_indices);
+ r_simplified_path_indices.push_back(point_max_index);
+ simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_simplified_path_indices);
+ }
+}
diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.h b/modules/navigation_2d/2d/nav_mesh_queries_2d.h
new file mode 100644
index 0000000000..d15913ce48
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.h
@@ -0,0 +1,141 @@
+/**************************************************************************/
+/* nav_mesh_queries_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+
+#include "servers/navigation/navigation_path_query_parameters_2d.h"
+#include "servers/navigation/navigation_path_query_result_2d.h"
+#include "servers/navigation/navigation_utilities.h"
+
+using namespace NavigationUtilities;
+
+class NavMap2D;
+struct NavMapIteration2D;
+
+class NavMeshQueries2D {
+public:
+ struct PathQuerySlot {
+ LocalVector path_corridor;
+ Heap traversable_polys;
+ bool in_use = false;
+ uint32_t slot_index = 0;
+ };
+
+ struct NavMeshPathQueryTask2D {
+ enum TaskStatus {
+ QUERY_STARTED,
+ QUERY_FINISHED,
+ QUERY_FAILED,
+ CALLBACK_DISPATCHED,
+ CALLBACK_FAILED,
+ };
+
+ // Parameters.
+ Vector2 start_position;
+ Vector2 target_position;
+ uint32_t navigation_layers;
+ BitField 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 excluded_regions;
+ LocalVector included_regions;
+
+ // Path building.
+ Vector2 begin_position;
+ Vector2 end_position;
+ const nav_2d::Polygon *begin_polygon = nullptr;
+ const nav_2d::Polygon *end_polygon = nullptr;
+ uint32_t least_cost_id = 0;
+
+ // Map.
+ NavMap2D *map = nullptr;
+ PathQuerySlot *path_query_slot = nullptr;
+
+ // Path points.
+ LocalVector path_points;
+ LocalVector path_meta_point_types;
+ LocalVector path_meta_point_rids;
+ LocalVector path_meta_point_owners;
+
+ Ref query_parameters;
+ Ref query_result;
+ Callable callback;
+ NavMeshPathQueryTask2D::TaskStatus status = NavMeshPathQueryTask2D::TaskStatus::QUERY_STARTED;
+
+ void path_clear() {
+ path_points.clear();
+ path_meta_point_types.clear();
+ path_meta_point_rids.clear();
+ path_meta_point_owners.clear();
+ }
+
+ void path_reverse() {
+ path_points.invert();
+ path_meta_point_types.invert();
+ path_meta_point_rids.invert();
+ path_meta_point_owners.invert();
+ }
+ };
+
+ static bool emit_callback(const Callable &p_callback);
+
+ static Vector2 polygons_get_random_point(const LocalVector &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
+
+ static Vector2 polygons_get_closest_point(const LocalVector &p_polygons, const Vector2 &p_point);
+ static nav_2d::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector &p_polygons, const Vector2 &p_point);
+ static RID polygons_get_closest_point_owner(const LocalVector &p_polygons, const Vector2 &p_point);
+
+ static Vector2 map_iteration_get_closest_point(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point);
+ static RID map_iteration_get_closest_point_owner(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point);
+ static nav_2d::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point);
+ static Vector2 map_iteration_get_random_point(const NavMapIteration2D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
+
+ static void map_query_path(NavMap2D *p_map, const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback);
+
+ static void query_task_map_iteration_get_path(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration);
+ static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask2D &p_query_task, const Vector2 &p_point, const nav_2d::Polygon *p_point_polygon);
+ static void _query_task_find_start_end_positions(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration);
+ static void _query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task);
+ static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask2D &p_query_task);
+ static void _query_task_post_process_edgecentered(NavMeshPathQueryTask2D &p_query_task);
+ static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask2D &p_query_task);
+ static void _query_task_clip_path(NavMeshPathQueryTask2D &p_query_task, const nav_2d::NavigationPoly *p_from_poly, const Vector2 &p_to_point, const nav_2d::NavigationPoly *p_to_poly);
+ static void _query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task);
+ static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner);
+
+ static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_simplified_path_indices);
+ static LocalVector get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon);
+};
diff --git a/modules/navigation_2d/2d/nav_region_iteration_2d.h b/modules/navigation_2d/2d/nav_region_iteration_2d.h
new file mode 100644
index 0000000000..4d50b36856
--- /dev/null
+++ b/modules/navigation_2d/2d/nav_region_iteration_2d.h
@@ -0,0 +1,46 @@
+/**************************************************************************/
+/* nav_region_iteration_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+#include "nav_base_iteration_2d.h"
+
+#include "core/math/rect2.h"
+
+struct NavRegionIteration2D : NavBaseIteration2D {
+ Transform2D transform;
+ real_t surface_area = 0.0;
+ Rect2 bounds;
+
+ const Transform2D &get_transform() const { return transform; }
+ real_t get_surface_area() const { return surface_area; }
+ Rect2 get_bounds() const { return bounds; }
+};
diff --git a/modules/navigation_2d/SCsub b/modules/navigation_2d/SCsub
new file mode 100644
index 0000000000..e3192f9e39
--- /dev/null
+++ b/modules/navigation_2d/SCsub
@@ -0,0 +1,42 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+Import("env_modules")
+
+env_navigation_2d = env_modules.Clone()
+
+# Thirdparty source files
+
+thirdparty_obj = []
+
+# RVO 2D Thirdparty source files
+if env["builtin_rvo2_2d"]:
+ thirdparty_dir = "#thirdparty/rvo2/rvo2_2d/"
+ thirdparty_sources = [
+ "Agent2d.cpp",
+ "Obstacle2d.cpp",
+ "KdTree2d.cpp",
+ "RVOSimulator2d.cpp",
+ ]
+ thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+ env_navigation_2d.Prepend(CPPPATH=[thirdparty_dir])
+
+ env_thirdparty = env_navigation_2d.Clone()
+ env_thirdparty.disable_warnings()
+ env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+
+
+env.modules_sources += thirdparty_obj
+
+# Godot source files
+
+module_obj = []
+
+env_navigation_2d.add_source_files(module_obj, "*.cpp")
+env_navigation_2d.add_source_files(module_obj, "2d/*.cpp")
+env.modules_sources += module_obj
+
+# Needed to force rebuilding the module files when the thirdparty library is updated.
+env.Depends(module_obj, thirdparty_obj)
diff --git a/modules/navigation_2d/config.py b/modules/navigation_2d/config.py
new file mode 100644
index 0000000000..d22f9454ed
--- /dev/null
+++ b/modules/navigation_2d/config.py
@@ -0,0 +1,6 @@
+def can_build(env, platform):
+ return True
+
+
+def configure(env):
+ pass
diff --git a/modules/navigation_2d/nav_agent_2d.cpp b/modules/navigation_2d/nav_agent_2d.cpp
new file mode 100644
index 0000000000..ee9deb62f4
--- /dev/null
+++ b/modules/navigation_2d/nav_agent_2d.cpp
@@ -0,0 +1,315 @@
+/**************************************************************************/
+/* nav_agent_2d.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#include "nav_agent_2d.h"
+
+#include "nav_map_2d.h"
+
+void NavAgent2D::set_avoidance_enabled(bool p_enabled) {
+ avoidance_enabled = p_enabled;
+ _update_rvo_agent_properties();
+}
+
+void NavAgent2D::_update_rvo_agent_properties() {
+ rvo_agent.neighborDist_ = neighbor_distance;
+ rvo_agent.maxNeighbors_ = max_neighbors;
+ rvo_agent.timeHorizon_ = time_horizon_agents;
+ rvo_agent.timeHorizonObst_ = time_horizon_obstacles;
+ rvo_agent.radius_ = radius;
+ rvo_agent.maxSpeed_ = max_speed;
+ rvo_agent.position_ = RVO2D::Vector2(position.x, position.y);
+ // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
+ //rvo_agent.velocity_ = RVO2D::Vector2(velocity.x, velocity.y);
+ rvo_agent.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.y);
+ rvo_agent.avoidance_layers_ = avoidance_layers;
+ rvo_agent.avoidance_mask_ = avoidance_mask;
+ rvo_agent.avoidance_priority_ = avoidance_priority;
+
+ if (map != nullptr) {
+ if (avoidance_enabled) {
+ map->set_agent_as_controlled(this);
+ } else {
+ map->remove_agent_as_controlled(this);
+ }
+ }
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_map(NavMap2D *p_map) {
+ if (map == p_map) {
+ return;
+ }
+
+ cancel_sync_request();
+
+ if (map) {
+ map->remove_agent(this);
+ }
+
+ map = p_map;
+ agent_dirty = true;
+
+ if (map) {
+ map->add_agent(this);
+ if (avoidance_enabled) {
+ map->set_agent_as_controlled(this);
+ }
+
+ request_sync();
+ }
+}
+
+bool NavAgent2D::is_map_changed() {
+ if (map) {
+ bool is_changed = map->get_iteration_id() != last_map_iteration_id;
+ last_map_iteration_id = map->get_iteration_id();
+ return is_changed;
+ } else {
+ return false;
+ }
+}
+
+void NavAgent2D::set_avoidance_callback(Callable p_callback) {
+ avoidance_callback = p_callback;
+}
+
+bool NavAgent2D::has_avoidance_callback() const {
+ return avoidance_callback.is_valid();
+}
+
+void NavAgent2D::dispatch_avoidance_callback() {
+ if (!avoidance_callback.is_valid()) {
+ return;
+ }
+
+ Vector3 new_velocity;
+
+ new_velocity = Vector3(rvo_agent.velocity_.x(), 0.0, rvo_agent.velocity_.y());
+
+ if (clamp_speed) {
+ new_velocity = new_velocity.limit_length(max_speed);
+ }
+
+ // Invoke the callback with the new velocity.
+ avoidance_callback.call(new_velocity);
+}
+
+void NavAgent2D::set_neighbor_distance(real_t p_neighbor_distance) {
+ neighbor_distance = p_neighbor_distance;
+ rvo_agent.neighborDist_ = neighbor_distance;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_max_neighbors(int p_max_neighbors) {
+ max_neighbors = p_max_neighbors;
+ rvo_agent.maxNeighbors_ = max_neighbors;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_time_horizon_agents(real_t p_time_horizon) {
+ time_horizon_agents = p_time_horizon;
+ rvo_agent.timeHorizon_ = time_horizon_agents;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_time_horizon_obstacles(real_t p_time_horizon) {
+ time_horizon_obstacles = p_time_horizon;
+ rvo_agent.timeHorizonObst_ = time_horizon_obstacles;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_radius(real_t p_radius) {
+ radius = p_radius;
+ rvo_agent.radius_ = radius;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_max_speed(real_t p_max_speed) {
+ max_speed = p_max_speed;
+ if (avoidance_enabled) {
+ rvo_agent.maxSpeed_ = max_speed;
+ }
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_position(const Vector2 &p_position) {
+ position = p_position;
+ if (avoidance_enabled) {
+ rvo_agent.position_ = RVO2D::Vector2(p_position.x, p_position.y);
+ }
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_target_position(const Vector2 &p_target_position) {
+ target_position = p_target_position;
+}
+
+void NavAgent2D::set_velocity(const Vector2 &p_velocity) {
+ // Sets the "wanted" velocity for an agent as a suggestion
+ // This velocity is not guaranteed, RVO simulation will only try to fulfill it
+ velocity = p_velocity;
+ if (avoidance_enabled) {
+ rvo_agent.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.y);
+ }
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_velocity_forced(const Vector2 &p_velocity) {
+ // This function replaces the internal rvo simulation velocity
+ // should only be used after the agent was teleported
+ // as it destroys consistency in movement in cramped situations
+ // use velocity instead to update with a safer "suggestion"
+ velocity_forced = p_velocity;
+ if (avoidance_enabled) {
+ rvo_agent.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.y);
+ }
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::update() {
+ // Updates this agent with the calculated results from the rvo simulation
+ if (avoidance_enabled) {
+ velocity = Vector2(rvo_agent.velocity_.x(), rvo_agent.velocity_.y());
+ }
+}
+
+void NavAgent2D::set_avoidance_mask(uint32_t p_mask) {
+ avoidance_mask = p_mask;
+ rvo_agent.avoidance_mask_ = avoidance_mask;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_avoidance_layers(uint32_t p_layers) {
+ avoidance_layers = p_layers;
+ rvo_agent.avoidance_layers_ = avoidance_layers;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+void NavAgent2D::set_avoidance_priority(real_t p_priority) {
+ ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+ ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+ avoidance_priority = p_priority;
+ rvo_agent.avoidance_priority_ = avoidance_priority;
+ agent_dirty = true;
+
+ request_sync();
+}
+
+bool NavAgent2D::is_dirty() const {
+ return agent_dirty;
+}
+
+void NavAgent2D::sync() {
+ agent_dirty = false;
+}
+
+const Dictionary NavAgent2D::get_avoidance_data() const {
+ // Returns debug data from RVO simulation internals of this agent.
+ Dictionary _avoidance_data;
+
+ _avoidance_data["max_neighbors"] = int(rvo_agent.maxNeighbors_);
+ _avoidance_data["max_speed"] = float(rvo_agent.maxSpeed_);
+ _avoidance_data["neighbor_distance"] = float(rvo_agent.neighborDist_);
+ _avoidance_data["new_velocity"] = Vector2(rvo_agent.newVelocity_.x(), rvo_agent.newVelocity_.y());
+ _avoidance_data["velocity"] = Vector2(rvo_agent.velocity_.x(), rvo_agent.velocity_.y());
+ _avoidance_data["position"] = Vector2(rvo_agent.position_.x(), rvo_agent.position_.y());
+ _avoidance_data["preferred_velocity"] = Vector2(rvo_agent.prefVelocity_.x(), rvo_agent.prefVelocity_.y());
+ _avoidance_data["radius"] = float(rvo_agent.radius_);
+ _avoidance_data["time_horizon_agents"] = float(rvo_agent.timeHorizon_);
+ _avoidance_data["time_horizon_obstacles"] = float(rvo_agent.timeHorizonObst_);
+ _avoidance_data["avoidance_layers"] = int(rvo_agent.avoidance_layers_);
+ _avoidance_data["avoidance_mask"] = int(rvo_agent.avoidance_mask_);
+ _avoidance_data["avoidance_priority"] = float(rvo_agent.avoidance_priority_);
+ return _avoidance_data;
+}
+
+void NavAgent2D::set_paused(bool p_paused) {
+ if (paused == p_paused) {
+ return;
+ }
+
+ paused = p_paused;
+
+ if (map) {
+ if (paused) {
+ map->remove_agent_as_controlled(this);
+ } else {
+ map->set_agent_as_controlled(this);
+ }
+ }
+}
+
+bool NavAgent2D::get_paused() const {
+ return paused;
+}
+
+void NavAgent2D::request_sync() {
+ if (map && !sync_dirty_request_list_element.in_list()) {
+ map->add_agent_sync_dirty_request(&sync_dirty_request_list_element);
+ }
+}
+
+void NavAgent2D::cancel_sync_request() {
+ if (map && sync_dirty_request_list_element.in_list()) {
+ map->remove_agent_sync_dirty_request(&sync_dirty_request_list_element);
+ }
+}
+
+NavAgent2D::NavAgent2D() :
+ sync_dirty_request_list_element(this) {
+}
+
+NavAgent2D::~NavAgent2D() {
+ cancel_sync_request();
+}
diff --git a/modules/navigation_2d/nav_agent_2d.h b/modules/navigation_2d/nav_agent_2d.h
new file mode 100644
index 0000000000..cc000f3698
--- /dev/null
+++ b/modules/navigation_2d/nav_agent_2d.h
@@ -0,0 +1,148 @@
+/**************************************************************************/
+/* nav_agent_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "nav_rid_2d.h"
+
+#include "core/object/class_db.h"
+#include "core/templates/self_list.h"
+
+#include
+
+class NavMap2D;
+
+class NavAgent2D : public NavRid2D {
+ Vector2 position;
+ Vector2 target_position;
+ Vector2 velocity;
+ Vector2 velocity_forced;
+ real_t radius = 1.0;
+ real_t max_speed = 1.0;
+ real_t time_horizon_agents = 1.0;
+ real_t time_horizon_obstacles = 0.0;
+ int max_neighbors = 5;
+ real_t neighbor_distance = 5.0;
+ Vector2 safe_velocity;
+ bool clamp_speed = true; // Experimental, clamps velocity to max_speed.
+
+ NavMap2D *map = nullptr;
+
+ RVO2D::Agent2D rvo_agent;
+ bool avoidance_enabled = false;
+
+ uint32_t avoidance_layers = 1;
+ uint32_t avoidance_mask = 1;
+ real_t avoidance_priority = 1.0;
+
+ Callable avoidance_callback;
+
+ bool agent_dirty = true;
+
+ uint32_t last_map_iteration_id = 0;
+ bool paused = false;
+
+ SelfList sync_dirty_request_list_element;
+
+public:
+ NavAgent2D();
+ ~NavAgent2D();
+
+ void set_avoidance_enabled(bool p_enabled);
+ bool is_avoidance_enabled() { return avoidance_enabled; }
+
+ void set_map(NavMap2D *p_map);
+ NavMap2D *get_map() { return map; }
+
+ bool is_map_changed();
+
+ RVO2D::Agent2D *get_rvo_agent() { return &rvo_agent; }
+
+ void set_avoidance_callback(Callable p_callback);
+ bool has_avoidance_callback() const;
+
+ void dispatch_avoidance_callback();
+
+ void set_neighbor_distance(real_t p_neighbor_distance);
+ real_t get_neighbor_distance() const { return neighbor_distance; }
+
+ void set_max_neighbors(int p_max_neighbors);
+ int get_max_neighbors() const { return max_neighbors; }
+
+ void set_time_horizon_agents(real_t p_time_horizon);
+ real_t get_time_horizon_agents() const { return time_horizon_agents; }
+
+ void set_time_horizon_obstacles(real_t p_time_horizon);
+ real_t get_time_horizon_obstacles() const { return time_horizon_obstacles; }
+
+ void set_radius(real_t p_radius);
+ real_t get_radius() const { return radius; }
+
+ void set_max_speed(real_t p_max_speed);
+ real_t get_max_speed() const { return max_speed; }
+
+ void set_position(const Vector2 &p_position);
+ Vector2 get_position() const { return position; }
+
+ void set_target_position(const Vector2 &p_target_position);
+ Vector2 get_target_position() const { return target_position; }
+
+ void set_velocity(const Vector2 &p_velocity);
+ Vector2 get_velocity() const { return velocity; }
+
+ void set_velocity_forced(const Vector2 &p_velocity);
+ Vector2 get_velocity_forced() const { return velocity_forced; }
+
+ void set_avoidance_layers(uint32_t p_layers);
+ uint32_t get_avoidance_layers() const { return avoidance_layers; }
+
+ void set_avoidance_mask(uint32_t p_mask);
+ uint32_t get_avoidance_mask() const { return avoidance_mask; }
+
+ void set_avoidance_priority(real_t p_priority);
+ real_t get_avoidance_priority() const { return avoidance_priority; }
+
+ void set_paused(bool p_paused);
+ bool get_paused() const;
+
+ bool is_dirty() const;
+ void sync();
+ void request_sync();
+ void cancel_sync_request();
+
+ // Updates this agent with rvo data after the rvo simulation avoidance step.
+ void update();
+
+ // RVO debug data from the last frame update.
+ const Dictionary get_avoidance_data() const;
+
+private:
+ void _update_rvo_agent_properties();
+};
diff --git a/modules/navigation_2d/nav_base_2d.h b/modules/navigation_2d/nav_base_2d.h
new file mode 100644
index 0000000000..ef0e19079f
--- /dev/null
+++ b/modules/navigation_2d/nav_base_2d.h
@@ -0,0 +1,67 @@
+/**************************************************************************/
+/* nav_base_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "nav_rid_2d.h"
+#include "nav_utils_2d.h"
+
+#include "servers/navigation/navigation_utilities.h"
+
+class NavMap2D;
+
+class NavBase2D : public NavRid2D {
+protected:
+ uint32_t navigation_layers = 1;
+ real_t enter_cost = 0.0;
+ real_t travel_cost = 1.0;
+ ObjectID owner_id;
+ NavigationUtilities::PathSegmentType type;
+
+public:
+ NavigationUtilities::PathSegmentType get_type() const { return type; }
+
+ virtual void set_use_edge_connections(bool p_enabled) {}
+ virtual bool get_use_edge_connections() const { return false; }
+
+ virtual void set_navigation_layers(uint32_t p_navigation_layers) {}
+ uint32_t get_navigation_layers() const { return navigation_layers; }
+
+ virtual void set_enter_cost(real_t p_enter_cost) {}
+ real_t get_enter_cost() const { return enter_cost; }
+
+ virtual void set_travel_cost(real_t p_travel_cost) {}
+ real_t get_travel_cost() const { return travel_cost; }
+
+ virtual void set_owner_id(ObjectID p_owner_id) {}
+ ObjectID get_owner_id() const { return owner_id; }
+
+ virtual ~NavBase2D() {}
+};
diff --git a/modules/navigation_2d/nav_link_2d.cpp b/modules/navigation_2d/nav_link_2d.cpp
new file mode 100644
index 0000000000..ecbc594e51
--- /dev/null
+++ b/modules/navigation_2d/nav_link_2d.cpp
@@ -0,0 +1,180 @@
+/**************************************************************************/
+/* nav_link_2d.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#include "nav_link_2d.h"
+
+#include "nav_map_2d.h"
+
+void NavLink2D::set_map(NavMap2D *p_map) {
+ if (map == p_map) {
+ return;
+ }
+
+ cancel_sync_request();
+
+ if (map) {
+ map->remove_link(this);
+ }
+
+ map = p_map;
+ link_dirty = true;
+
+ if (map) {
+ map->add_link(this);
+ request_sync();
+ }
+}
+
+void NavLink2D::set_enabled(bool p_enabled) {
+ if (enabled == p_enabled) {
+ return;
+ }
+ enabled = p_enabled;
+
+ // TODO: This should not require a full rebuild as the link has not really changed.
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_bidirectional(bool p_bidirectional) {
+ if (bidirectional == p_bidirectional) {
+ return;
+ }
+ bidirectional = p_bidirectional;
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_start_position(const Vector2 &p_position) {
+ if (start_position == p_position) {
+ return;
+ }
+ start_position = p_position;
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_end_position(const Vector2 &p_position) {
+ if (end_position == p_position) {
+ return;
+ }
+ end_position = p_position;
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_navigation_layers(uint32_t p_navigation_layers) {
+ if (navigation_layers == p_navigation_layers) {
+ return;
+ }
+ navigation_layers = p_navigation_layers;
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_enter_cost(real_t p_enter_cost) {
+ real_t new_enter_cost = MAX(p_enter_cost, 0.0);
+ if (enter_cost == new_enter_cost) {
+ return;
+ }
+ enter_cost = new_enter_cost;
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_travel_cost(real_t p_travel_cost) {
+ real_t new_travel_cost = MAX(p_travel_cost, 0.0);
+ if (travel_cost == new_travel_cost) {
+ return;
+ }
+ travel_cost = new_travel_cost;
+ link_dirty = true;
+
+ request_sync();
+}
+
+void NavLink2D::set_owner_id(ObjectID p_owner_id) {
+ if (owner_id == p_owner_id) {
+ return;
+ }
+ owner_id = p_owner_id;
+ link_dirty = true;
+
+ request_sync();
+}
+
+bool NavLink2D::is_dirty() const {
+ return link_dirty;
+}
+
+void NavLink2D::sync() {
+ link_dirty = false;
+}
+
+void NavLink2D::request_sync() {
+ if (map && !sync_dirty_request_list_element.in_list()) {
+ map->add_link_sync_dirty_request(&sync_dirty_request_list_element);
+ }
+}
+
+void NavLink2D::cancel_sync_request() {
+ if (map && sync_dirty_request_list_element.in_list()) {
+ map->remove_link_sync_dirty_request(&sync_dirty_request_list_element);
+ }
+}
+
+NavLink2D::NavLink2D() :
+ sync_dirty_request_list_element(this) {
+ type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
+}
+
+NavLink2D::~NavLink2D() {
+ cancel_sync_request();
+}
+
+void NavLink2D::get_iteration_update(NavLinkIteration2D &r_iteration) {
+ r_iteration.navigation_layers = get_navigation_layers();
+ r_iteration.enter_cost = get_enter_cost();
+ r_iteration.travel_cost = get_travel_cost();
+ r_iteration.owner_object_id = get_owner_id();
+ r_iteration.owner_type = get_type();
+ r_iteration.owner_rid = get_self();
+
+ r_iteration.enabled = get_enabled();
+ r_iteration.start_position = get_start_position();
+ r_iteration.end_position = get_end_position();
+ r_iteration.bidirectional = is_bidirectional();
+}
diff --git a/modules/navigation_2d/nav_link_2d.h b/modules/navigation_2d/nav_link_2d.h
new file mode 100644
index 0000000000..1d324e1e2b
--- /dev/null
+++ b/modules/navigation_2d/nav_link_2d.h
@@ -0,0 +1,99 @@
+/**************************************************************************/
+/* nav_link_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "2d/nav_base_iteration_2d.h"
+#include "nav_base_2d.h"
+#include "nav_utils_2d.h"
+
+struct NavLinkIteration2D : NavBaseIteration2D {
+ bool bidirectional = true;
+ Vector2 start_position;
+ Vector2 end_position;
+
+ Vector2 get_start_position() const { return start_position; }
+ Vector2 get_end_position() const { return end_position; }
+ bool is_bidirectional() const { return bidirectional; }
+};
+
+#include "core/templates/self_list.h"
+
+class NavLink2D : public NavBase2D {
+ NavMap2D *map = nullptr;
+ bool bidirectional = true;
+ Vector2 start_position;
+ Vector2 end_position;
+ bool enabled = true;
+
+ bool link_dirty = true;
+
+ SelfList sync_dirty_request_list_element;
+
+public:
+ NavLink2D();
+ ~NavLink2D();
+
+ void set_map(NavMap2D *p_map);
+ NavMap2D *get_map() const {
+ return map;
+ }
+
+ void set_enabled(bool p_enabled);
+ bool get_enabled() const { return enabled; }
+
+ void set_bidirectional(bool p_bidirectional);
+ bool is_bidirectional() const {
+ return bidirectional;
+ }
+
+ void set_start_position(const Vector2 &p_position);
+ Vector2 get_start_position() const {
+ return start_position;
+ }
+
+ void set_end_position(const Vector2 &p_position);
+ Vector2 get_end_position() const {
+ return end_position;
+ }
+
+ // NavBase properties.
+ virtual void set_navigation_layers(uint32_t p_navigation_layers) override;
+ virtual void set_enter_cost(real_t p_enter_cost) override;
+ virtual void set_travel_cost(real_t p_travel_cost) override;
+ virtual void set_owner_id(ObjectID p_owner_id) override;
+
+ bool is_dirty() const;
+ void sync();
+ void request_sync();
+ void cancel_sync_request();
+
+ void get_iteration_update(NavLinkIteration2D &r_iteration);
+};
diff --git a/modules/navigation_2d/nav_map_2d.cpp b/modules/navigation_2d/nav_map_2d.cpp
new file mode 100644
index 0000000000..285462e044
--- /dev/null
+++ b/modules/navigation_2d/nav_map_2d.cpp
@@ -0,0 +1,780 @@
+/**************************************************************************/
+/* nav_map_2d.cpp */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#include "nav_map_2d.h"
+
+#include "2d/nav_map_builder_2d.h"
+#include "2d/nav_mesh_queries_2d.h"
+#include "2d/nav_region_iteration_2d.h"
+#include "nav_agent_2d.h"
+#include "nav_link_2d.h"
+#include "nav_obstacle_2d.h"
+#include "nav_region_2d.h"
+
+#include "core/config/project_settings.h"
+#include "core/object/worker_thread_pool.h"
+
+#include
+
+using namespace nav_2d;
+
+#ifdef DEBUG_ENABLED
+#define NAVMAP_ITERATION_ZERO_ERROR_MSG() \
+ ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\
+ NavigationServer 'map_changed' signal can be used to receive update notifications.\n\
+ NavigationServer 'map_get_iteration_id()' can be used to check if a map has finished its newest iteration.");
+#else
+#define NAVMAP_ITERATION_ZERO_ERROR_MSG()
+#endif // DEBUG_ENABLED
+
+#define GET_MAP_ITERATION() \
+ iteration_slot_rwlock.read_lock(); \
+ NavMapIteration2D &map_iteration = iteration_slots[iteration_slot_index]; \
+ NavMapIterationRead2D iteration_read_lock(map_iteration); \
+ iteration_slot_rwlock.read_unlock();
+
+#define GET_MAP_ITERATION_CONST() \
+ iteration_slot_rwlock.read_lock(); \
+ const NavMapIteration2D &map_iteration = iteration_slots[iteration_slot_index]; \
+ NavMapIterationRead2D iteration_read_lock(map_iteration); \
+ iteration_slot_rwlock.read_unlock();
+
+void NavMap2D::set_cell_size(real_t p_cell_size) {
+ if (cell_size == p_cell_size) {
+ return;
+ }
+ cell_size = MAX(p_cell_size, NavigationDefaults2D::navmesh_cell_size_min);
+ _update_merge_rasterizer_cell_dimensions();
+ map_settings_dirty = true;
+}
+
+void NavMap2D::set_merge_rasterizer_cell_scale(float p_value) {
+ if (merge_rasterizer_cell_scale == p_value) {
+ return;
+ }
+ merge_rasterizer_cell_scale = MAX(p_value, NavigationDefaults2D::navmesh_cell_size_min);
+ _update_merge_rasterizer_cell_dimensions();
+ map_settings_dirty = true;
+}
+
+void NavMap2D::set_use_edge_connections(bool p_enabled) {
+ if (use_edge_connections == p_enabled) {
+ return;
+ }
+ use_edge_connections = p_enabled;
+ iteration_dirty = true;
+}
+
+void NavMap2D::set_edge_connection_margin(real_t p_edge_connection_margin) {
+ if (edge_connection_margin == p_edge_connection_margin) {
+ return;
+ }
+ edge_connection_margin = p_edge_connection_margin;
+ iteration_dirty = true;
+}
+
+void NavMap2D::set_link_connection_radius(real_t p_link_connection_radius) {
+ if (link_connection_radius == p_link_connection_radius) {
+ return;
+ }
+ link_connection_radius = p_link_connection_radius;
+ iteration_dirty = true;
+}
+
+Vector2 NavMap2D::get_merge_rasterizer_cell_size() const {
+ return merge_rasterizer_cell_size;
+}
+
+PointKey NavMap2D::get_point_key(const Vector2 &p_pos) const {
+ const int x = static_cast(Math::floor(p_pos.x / merge_rasterizer_cell_size.x));
+ const int y = static_cast(Math::floor(p_pos.y / merge_rasterizer_cell_size.y));
+
+ PointKey p;
+ p.key = 0;
+ p.x = x;
+ p.y = y;
+ return p;
+}
+
+void NavMap2D::query_path(NavMeshQueries2D::NavMeshPathQueryTask2D &p_query_task) {
+ if (iteration_id == 0) {
+ return;
+ }
+
+ GET_MAP_ITERATION();
+
+ map_iteration.path_query_slots_semaphore.wait();
+
+ map_iteration.path_query_slots_mutex.lock();
+ for (NavMeshQueries2D::PathQuerySlot &p_path_query_slot : map_iteration.path_query_slots) {
+ if (!p_path_query_slot.in_use) {
+ p_path_query_slot.in_use = true;
+ p_query_task.path_query_slot = &p_path_query_slot;
+ break;
+ }
+ }
+ map_iteration.path_query_slots_mutex.unlock();
+
+ if (p_query_task.path_query_slot == nullptr) {
+ map_iteration.path_query_slots_semaphore.post();
+ ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap2D path query slot found! This should never happen :(.");
+ }
+
+ NavMeshQueries2D::query_task_map_iteration_get_path(p_query_task, map_iteration);
+
+ map_iteration.path_query_slots_mutex.lock();
+ uint32_t used_slot_index = p_query_task.path_query_slot->slot_index;
+ map_iteration.path_query_slots[used_slot_index].in_use = false;
+ p_query_task.path_query_slot = nullptr;
+ map_iteration.path_query_slots_mutex.unlock();
+
+ map_iteration.path_query_slots_semaphore.post();
+}
+
+Vector2 NavMap2D::get_closest_point(const Vector2 &p_point) const {
+ if (iteration_id == 0) {
+ NAVMAP_ITERATION_ZERO_ERROR_MSG();
+ return Vector2();
+ }
+
+ GET_MAP_ITERATION_CONST();
+
+ return NavMeshQueries2D::map_iteration_get_closest_point(map_iteration, p_point);
+}
+
+RID NavMap2D::get_closest_point_owner(const Vector2 &p_point) const {
+ if (iteration_id == 0) {
+ NAVMAP_ITERATION_ZERO_ERROR_MSG();
+ return RID();
+ }
+
+ GET_MAP_ITERATION_CONST();
+
+ return NavMeshQueries2D::map_iteration_get_closest_point_owner(map_iteration, p_point);
+}
+
+ClosestPointQueryResult NavMap2D::get_closest_point_info(const Vector2 &p_point) const {
+ GET_MAP_ITERATION_CONST();
+
+ return NavMeshQueries2D::map_iteration_get_closest_point_info(map_iteration, p_point);
+}
+
+void NavMap2D::add_region(NavRegion2D *p_region) {
+ regions.push_back(p_region);
+ iteration_dirty = true;
+}
+
+void NavMap2D::remove_region(NavRegion2D *p_region) {
+ int64_t region_index = regions.find(p_region);
+ if (region_index >= 0) {
+ regions.remove_at_unordered(region_index);
+ iteration_dirty = true;
+ }
+}
+
+void NavMap2D::add_link(NavLink2D *p_link) {
+ links.push_back(p_link);
+ iteration_dirty = true;
+}
+
+void NavMap2D::remove_link(NavLink2D *p_link) {
+ int64_t link_index = links.find(p_link);
+ if (link_index >= 0) {
+ links.remove_at_unordered(link_index);
+ iteration_dirty = true;
+ }
+}
+
+bool NavMap2D::has_agent(NavAgent2D *p_agent) const {
+ return agents.has(p_agent);
+}
+
+void NavMap2D::add_agent(NavAgent2D *p_agent) {
+ if (!has_agent(p_agent)) {
+ agents.push_back(p_agent);
+ agents_dirty = true;
+ }
+}
+
+void NavMap2D::remove_agent(NavAgent2D *p_agent) {
+ remove_agent_as_controlled(p_agent);
+ int64_t agent_index = agents.find(p_agent);
+ if (agent_index >= 0) {
+ agents.remove_at_unordered(agent_index);
+ agents_dirty = true;
+ }
+}
+
+bool NavMap2D::has_obstacle(NavObstacle2D *p_obstacle) const {
+ return obstacles.has(p_obstacle);
+}
+
+void NavMap2D::add_obstacle(NavObstacle2D *p_obstacle) {
+ if (p_obstacle->get_paused()) {
+ // No point in adding a paused obstacle, it will add itself when unpaused again.
+ return;
+ }
+
+ if (!has_obstacle(p_obstacle)) {
+ obstacles.push_back(p_obstacle);
+ obstacles_dirty = true;
+ }
+}
+
+void NavMap2D::remove_obstacle(NavObstacle2D *p_obstacle) {
+ int64_t obstacle_index = obstacles.find(p_obstacle);
+ if (obstacle_index >= 0) {
+ obstacles.remove_at_unordered(obstacle_index);
+ obstacles_dirty = true;
+ }
+}
+
+void NavMap2D::set_agent_as_controlled(NavAgent2D *p_agent) {
+ remove_agent_as_controlled(p_agent);
+
+ if (p_agent->get_paused()) {
+ // No point in adding a paused agent, it will add itself when unpaused again.
+ return;
+ }
+
+ int64_t agent_index = active_avoidance_agents.find(p_agent);
+ if (agent_index < 0) {
+ active_avoidance_agents.push_back(p_agent);
+ agents_dirty = true;
+ }
+}
+
+void NavMap2D::remove_agent_as_controlled(NavAgent2D *p_agent) {
+ int64_t agent_index = active_avoidance_agents.find(p_agent);
+ if (agent_index >= 0) {
+ active_avoidance_agents.remove_at_unordered(agent_index);
+ agents_dirty = true;
+ }
+}
+
+Vector2 NavMap2D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+ GET_MAP_ITERATION_CONST();
+
+ return NavMeshQueries2D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly);
+}
+
+void NavMap2D::_build_iteration() {
+ if (!iteration_dirty || iteration_building || iteration_ready) {
+ return;
+ }
+
+ // Get the next free iteration slot that should be potentially unused.
+ iteration_slot_rwlock.read_lock();
+ NavMapIteration2D &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2];
+ // Check if the iteration slot is truly free or still used by an external thread.
+ bool iteration_is_free = next_map_iteration.users.get() == 0;
+ iteration_slot_rwlock.read_unlock();
+
+ if (!iteration_is_free) {
+ // A long running pathfinding thread or something is still reading
+ // from this older iteration and needs to finish first.
+ // Return and wait for the next sync cycle to check again.
+ return;
+ }
+
+ // Iteration slot is free and no longer used by anything, let's build.
+
+ iteration_dirty = false;
+ iteration_building = true;
+ iteration_ready = false;
+
+ // We don't need to hold any lock because at this point nothing else can touch it.
+ // All new queries are already forwarded to the other iteration slot.
+
+ iteration_build.reset();
+
+ iteration_build.merge_rasterizer_cell_size = get_merge_rasterizer_cell_size();
+ iteration_build.use_edge_connections = get_use_edge_connections();
+ iteration_build.edge_connection_margin = get_edge_connection_margin();
+ iteration_build.link_connection_radius = get_link_connection_radius();
+
+ uint32_t enabled_region_count = 0;
+ uint32_t enabled_link_count = 0;
+
+ for (NavRegion2D *region : regions) {
+ if (!region->get_enabled()) {
+ continue;
+ }
+ enabled_region_count++;
+ }
+ for (NavLink2D *link : links) {
+ if (!link->get_enabled()) {
+ continue;
+ }
+ enabled_link_count++;
+ }
+
+ next_map_iteration.region_ptr_to_region_id.clear();
+
+ next_map_iteration.region_iterations.clear();
+ next_map_iteration.link_iterations.clear();
+
+ next_map_iteration.region_iterations.resize(enabled_region_count);
+ next_map_iteration.link_iterations.resize(enabled_link_count);
+
+ uint32_t region_id_count = 0;
+ uint32_t link_id_count = 0;
+
+ for (NavRegion2D *region : regions) {
+ if (!region->get_enabled()) {
+ continue;
+ }
+ NavRegionIteration2D ®ion_iteration = next_map_iteration.region_iterations[region_id_count];
+ region_iteration.id = region_id_count++;
+ region->get_iteration_update(region_iteration);
+ next_map_iteration.region_ptr_to_region_id[region] = (uint32_t)region_iteration.id;
+ }
+ for (NavLink2D *link : links) {
+ if (!link->get_enabled()) {
+ continue;
+ }
+ NavLinkIteration2D &link_iteration = next_map_iteration.link_iterations[link_id_count];
+ link_iteration.id = link_id_count++;
+ link->get_iteration_update(link_iteration);
+ }
+
+ iteration_build.map_iteration = &next_map_iteration;
+
+ if (use_async_iterations) {
+ iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap2D::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder2D"));
+ } else {
+ NavMapBuilder2D::build_navmap_iteration(iteration_build);
+
+ iteration_building = false;
+ iteration_ready = true;
+ }
+}
+
+void NavMap2D::_build_iteration_threaded(void *p_arg) {
+ NavMapIterationBuild2D *_iteration_build = static_cast(p_arg);
+
+ NavMapBuilder2D::build_navmap_iteration(*_iteration_build);
+}
+
+void NavMap2D::_sync_iteration() {
+ if (iteration_building || !iteration_ready) {
+ return;
+ }
+
+ performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count;
+ performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count;
+ performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count;
+ performance_data.pm_edge_connection_count = iteration_build.performance_data.pm_edge_connection_count;
+ performance_data.pm_edge_free_count = iteration_build.performance_data.pm_edge_free_count;
+
+ iteration_id = iteration_id % UINT32_MAX + 1;
+
+ // Finally ping-pong switch the iteration slot.
+ iteration_slot_rwlock.write_lock();
+ uint32_t next_iteration_slot_index = (iteration_slot_index + 1) % 2;
+ iteration_slot_index = next_iteration_slot_index;
+ iteration_slot_rwlock.write_unlock();
+
+ iteration_ready = false;
+}
+
+void NavMap2D::sync() {
+ // Performance Monitor.
+ performance_data.pm_region_count = regions.size();
+ performance_data.pm_agent_count = agents.size();
+ performance_data.pm_link_count = links.size();
+ performance_data.pm_obstacle_count = obstacles.size();
+
+ _sync_dirty_map_update_requests();
+
+ if (iteration_dirty && !iteration_building && !iteration_ready) {
+ _build_iteration();
+ }
+ if (use_async_iterations && iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+ if (WorkerThreadPool::get_singleton()->is_task_completed(iteration_build_thread_task_id)) {
+ WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
+
+ iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+ iteration_building = false;
+ iteration_ready = true;
+ }
+ }
+ if (iteration_ready) {
+ _sync_iteration();
+ }
+
+ map_settings_dirty = false;
+
+ _sync_avoidance();
+}
+
+void NavMap2D::_sync_avoidance() {
+ _sync_dirty_avoidance_update_requests();
+
+ if (obstacles_dirty || agents_dirty) {
+ _update_rvo_simulation();
+ }
+
+ obstacles_dirty = false;
+ agents_dirty = false;
+}
+
+void NavMap2D::_update_rvo_obstacles_tree() {
+ int obstacle_vertex_count = 0;
+ for (NavObstacle2D *obstacle : obstacles) {
+ obstacle_vertex_count += obstacle->get_vertices().size();
+ }
+
+ // Cleaning old obstacles.
+ for (size_t i = 0; i < rvo_simulation.obstacles_.size(); ++i) {
+ delete rvo_simulation.obstacles_[i];
+ }
+ rvo_simulation.obstacles_.clear();
+
+ // Cannot use LocalVector here as RVO library expects std::vector to build KdTree
+ std::vector &raw_obstacles = rvo_simulation.obstacles_;
+ raw_obstacles.reserve(obstacle_vertex_count);
+
+ // The following block is modified copy from RVO2D::AddObstacle()
+ // Obstacles are linked and depend on all other obstacles.
+ for (NavObstacle2D *obstacle : obstacles) {
+ const Vector2 &_obstacle_position = obstacle->get_position();
+ const Vector &_obstacle_vertices = obstacle->get_vertices();
+
+ if (_obstacle_vertices.size() < 2) {
+ continue;
+ }
+
+ std::vector rvo_vertices;
+ rvo_vertices.reserve(_obstacle_vertices.size());
+
+ uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
+
+ for (const Vector2 &_obstacle_vertex : _obstacle_vertices) {
+ rvo_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.y + _obstacle_position.y));
+ }
+
+ const size_t obstacleNo = raw_obstacles.size();
+
+ for (size_t i = 0; i < rvo_vertices.size(); i++) {
+ RVO2D::Obstacle2D *rvo_obstacle = new RVO2D::Obstacle2D();
+ rvo_obstacle->point_ = rvo_vertices[i];
+
+ rvo_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
+
+ if (i != 0) {
+ rvo_obstacle->prevObstacle_ = raw_obstacles.back();
+ rvo_obstacle->prevObstacle_->nextObstacle_ = rvo_obstacle;
+ }
+
+ if (i == rvo_vertices.size() - 1) {
+ rvo_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
+ rvo_obstacle->nextObstacle_->prevObstacle_ = rvo_obstacle;
+ }
+
+ rvo_obstacle->unitDir_ = normalize(rvo_vertices[(i == rvo_vertices.size() - 1 ? 0 : i + 1)] - rvo_vertices[i]);
+
+ if (rvo_vertices.size() == 2) {
+ rvo_obstacle->isConvex_ = true;
+ } else {
+ rvo_obstacle->isConvex_ = (leftOf(rvo_vertices[(i == 0 ? rvo_vertices.size() - 1 : i - 1)], rvo_vertices[i], rvo_vertices[(i == rvo_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
+ }
+
+ rvo_obstacle->id_ = raw_obstacles.size();
+
+ raw_obstacles.push_back(rvo_obstacle);
+ }
+ }
+
+ rvo_simulation.kdTree_->buildObstacleTree(raw_obstacles);
+}
+
+void NavMap2D::_update_rvo_agents_tree() {
+ // Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
+ std::vector raw_agents;
+ raw_agents.reserve(active_avoidance_agents.size());
+ for (NavAgent2D *agent : active_avoidance_agents) {
+ raw_agents.push_back(agent->get_rvo_agent());
+ }
+ rvo_simulation.kdTree_->buildAgentTree(raw_agents);
+}
+
+void NavMap2D::_update_rvo_simulation() {
+ if (obstacles_dirty) {
+ _update_rvo_obstacles_tree();
+ }
+ if (agents_dirty) {
+ _update_rvo_agents_tree();
+ }
+}
+
+void NavMap2D::compute_single_avoidance_step(uint32_t p_index, NavAgent2D **p_agent) {
+ (*(p_agent + p_index))->get_rvo_agent()->computeNeighbors(&rvo_simulation);
+ (*(p_agent + p_index))->get_rvo_agent()->computeNewVelocity(&rvo_simulation);
+ (*(p_agent + p_index))->get_rvo_agent()->update(&rvo_simulation);
+ (*(p_agent + p_index))->update();
+}
+
+void NavMap2D::step(real_t p_deltatime) {
+ deltatime = p_deltatime;
+
+ rvo_simulation.setTimeStep(float(deltatime));
+
+ if (active_avoidance_agents.size() > 0) {
+ if (use_threads && avoidance_use_multiple_threads) {
+ WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap2D::compute_single_avoidance_step, active_avoidance_agents.ptr(), active_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
+ WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+ } else {
+ for (NavAgent2D *agent : active_avoidance_agents) {
+ agent->get_rvo_agent()->computeNeighbors(&rvo_simulation);
+ agent->get_rvo_agent()->computeNewVelocity(&rvo_simulation);
+ agent->get_rvo_agent()->update(&rvo_simulation);
+ agent->update();
+ }
+ }
+ }
+}
+
+void NavMap2D::dispatch_callbacks() {
+ for (NavAgent2D *agent : active_avoidance_agents) {
+ agent->dispatch_avoidance_callback();
+ }
+}
+
+void NavMap2D::_update_merge_rasterizer_cell_dimensions() {
+ merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale;
+ merge_rasterizer_cell_size.y = cell_size * merge_rasterizer_cell_scale;
+}
+
+int NavMap2D::get_region_connections_count(NavRegion2D *p_region) const {
+ ERR_FAIL_NULL_V(p_region, 0);
+
+ GET_MAP_ITERATION_CONST();
+
+ HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+ if (found_id) {
+ HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+ if (found_connections) {
+ return found_connections->value.size();
+ }
+ }
+
+ return 0;
+}
+
+Vector2 NavMap2D::get_region_connection_pathway_start(NavRegion2D *p_region, int p_connection_id) const {
+ ERR_FAIL_NULL_V(p_region, Vector2());
+
+ GET_MAP_ITERATION_CONST();
+
+ HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+ if (found_id) {
+ HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+ if (found_connections) {
+ ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector2());
+ return found_connections->value[p_connection_id].pathway_start;
+ }
+ }
+
+ return Vector2();
+}
+
+Vector2 NavMap2D::get_region_connection_pathway_end(NavRegion2D *p_region, int p_connection_id) const {
+ ERR_FAIL_NULL_V(p_region, Vector2());
+
+ GET_MAP_ITERATION_CONST();
+
+ HashMap::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+ if (found_id) {
+ HashMap>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+ if (found_connections) {
+ ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector2());
+ return found_connections->value[p_connection_id].pathway_end;
+ }
+ }
+
+ return Vector2();
+}
+
+void NavMap2D::add_region_sync_dirty_request(SelfList *p_sync_request) {
+ if (p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.regions.add(p_sync_request);
+}
+
+void NavMap2D::add_link_sync_dirty_request(SelfList *p_sync_request) {
+ if (p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.links.add(p_sync_request);
+}
+
+void NavMap2D::add_agent_sync_dirty_request(SelfList *p_sync_request) {
+ if (p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.agents.add(p_sync_request);
+}
+
+void NavMap2D::add_obstacle_sync_dirty_request(SelfList *p_sync_request) {
+ if (p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.obstacles.add(p_sync_request);
+}
+
+void NavMap2D::remove_region_sync_dirty_request(SelfList *p_sync_request) {
+ if (!p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.regions.remove(p_sync_request);
+}
+
+void NavMap2D::remove_link_sync_dirty_request(SelfList *p_sync_request) {
+ if (!p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.links.remove(p_sync_request);
+}
+
+void NavMap2D::remove_agent_sync_dirty_request(SelfList *p_sync_request) {
+ if (!p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.agents.remove(p_sync_request);
+}
+
+void NavMap2D::remove_obstacle_sync_dirty_request(SelfList *p_sync_request) {
+ if (!p_sync_request->in_list()) {
+ return;
+ }
+ sync_dirty_requests.obstacles.remove(p_sync_request);
+}
+
+void NavMap2D::_sync_dirty_map_update_requests() {
+ // If entire map settings changed make all regions dirty.
+ if (map_settings_dirty) {
+ for (NavRegion2D *region : regions) {
+ region->scratch_polygons();
+ }
+ iteration_dirty = true;
+ }
+
+ if (!iteration_dirty) {
+ iteration_dirty = sync_dirty_requests.regions.first() || sync_dirty_requests.links.first();
+ }
+
+ // Sync NavRegions.
+ for (SelfList *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
+ element->self()->sync();
+ }
+ sync_dirty_requests.regions.clear();
+
+ // Sync NavLinks.
+ for (SelfList *element = sync_dirty_requests.links.first(); element; element = element->next()) {
+ element->self()->sync();
+ }
+ sync_dirty_requests.links.clear();
+}
+
+void NavMap2D::_sync_dirty_avoidance_update_requests() {
+ // Sync NavAgents.
+ if (!agents_dirty) {
+ agents_dirty = sync_dirty_requests.agents.first();
+ }
+ for (SelfList *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
+ element->self()->sync();
+ }
+ sync_dirty_requests.agents.clear();
+
+ // Sync NavObstacles.
+ if (!obstacles_dirty) {
+ obstacles_dirty = sync_dirty_requests.obstacles.first();
+ }
+ for (SelfList *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
+ element->self()->sync();
+ }
+ sync_dirty_requests.obstacles.clear();
+}
+
+void NavMap2D::set_use_async_iterations(bool p_enabled) {
+ if (use_async_iterations == p_enabled) {
+ return;
+ }
+#ifdef THREADS_ENABLED
+ use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavMap2D::get_use_async_iterations() const {
+ return use_async_iterations;
+}
+
+NavMap2D::NavMap2D() {
+ avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
+ avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
+
+ path_query_slots_max = GLOBAL_GET("navigation/pathfinding/max_threads");
+
+ int processor_count = OS::get_singleton()->get_processor_count();
+ if (path_query_slots_max < 0) {
+ path_query_slots_max = processor_count;
+ }
+ if (processor_count < path_query_slots_max) {
+ path_query_slots_max = processor_count;
+ }
+ if (path_query_slots_max < 1) {
+ path_query_slots_max = 1;
+ }
+
+ iteration_slots.resize(2);
+
+ for (NavMapIteration2D &iteration_slot : iteration_slots) {
+ iteration_slot.path_query_slots.resize(path_query_slots_max);
+ for (uint32_t i = 0; i < iteration_slot.path_query_slots.size(); i++) {
+ iteration_slot.path_query_slots[i].slot_index = i;
+ }
+ iteration_slot.path_query_slots_semaphore.post(path_query_slots_max);
+ }
+
+#ifdef THREADS_ENABLED
+ use_async_iterations = GLOBAL_GET("navigation/world/map_use_async_iterations");
+#else
+ use_async_iterations = false;
+#endif
+}
+
+NavMap2D::~NavMap2D() {
+ if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+ WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
+ iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+ }
+}
diff --git a/modules/navigation_2d/nav_map_2d.h b/modules/navigation_2d/nav_map_2d.h
new file mode 100644
index 0000000000..584c24ae05
--- /dev/null
+++ b/modules/navigation_2d/nav_map_2d.h
@@ -0,0 +1,251 @@
+/**************************************************************************/
+/* nav_map_2d.h */
+/**************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/**************************************************************************/
+
+#pragma once
+
+#include "2d/nav_map_iteration_2d.h"
+#include "2d/nav_mesh_queries_2d.h"
+#include "nav_rid_2d.h"
+#include "nav_utils_2d.h"
+
+#include "core/math/math_defs.h"
+#include "core/object/worker_thread_pool.h"
+#include "servers/navigation/navigation_globals.h"
+
+#include
+#include
+
+class NavLink2D;
+class NavRegion2D;
+class NavAgent2D;
+class NavObstacle2D;
+
+class NavMap2D : public NavRid2D {
+ /// To find the polygons edges the vertices are displaced in a grid where
+ /// each cell has the following cell_size.
+ real_t cell_size = NavigationDefaults2D::navmesh_cell_size;
+
+ // For the inter-region merging to work, internal rasterization is performed.
+ Vector2 merge_rasterizer_cell_size = Vector2(cell_size, cell_size);
+
+ // This value is used to control sensitivity of internal rasterizer.
+ float merge_rasterizer_cell_scale = 1.0;
+
+ bool use_edge_connections = true;
+ /// This value is used to detect the near edges to connect.
+ real_t edge_connection_margin = NavigationDefaults2D::edge_connection_margin;
+
+ /// This value is used to limit how far links search to find polygons to connect to.
+ real_t link_connection_radius = NavigationDefaults2D::link_connection_radius;
+
+ bool map_settings_dirty = true;
+
+ /// Map regions.
+ LocalVector regions;
+
+ /// Map links.
+ LocalVector links;
+
+ /// RVO avoidance world.
+ RVO2D::RVOSimulator2D rvo_simulation;
+
+ /// Avoidance controlled agents.
+ LocalVector active_avoidance_agents;
+
+ /// dirty flag when one of the agent's arrays are modified.
+ bool agents_dirty = true;
+
+ /// All the Agents (even the controlled one).
+ LocalVector agents;
+
+ /// All the avoidance obstacles (both static and dynamic).
+ LocalVector obstacles;
+
+ /// Are rvo obstacles modified?
+ bool obstacles_dirty = true;
+
+ /// Physics delta time.
+ real_t deltatime = 0.0;
+
+ /// Change the id each time the map is updated.
+ uint32_t iteration_id = 0;
+
+ bool use_threads = true;
+ bool avoidance_use_multiple_threads = true;
+ bool avoidance_use_high_priority_threads = true;
+
+ // Performance Monitor
+ nav_2d::PerformanceData performance_data;
+
+ struct {
+ SelfList::List regions;
+ SelfList::List links;
+ SelfList::List agents;
+ SelfList::List obstacles;
+ } sync_dirty_requests;
+
+ int path_query_slots_max = 4;
+
+ bool use_async_iterations = true;
+
+ uint32_t iteration_slot_index = 0;
+ LocalVector iteration_slots;
+ mutable RWLock iteration_slot_rwlock;
+
+ NavMapIterationBuild2D iteration_build;
+ bool iteration_build_use_threads = false;
+ WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+ static void _build_iteration_threaded(void *p_arg);
+
+ bool iteration_dirty = true;
+ bool iteration_building = false;
+ bool iteration_ready = false;
+
+ void _build_iteration();
+ void _sync_iteration();
+
+public:
+ NavMap2D();
+ ~NavMap2D();
+
+ uint32_t get_iteration_id() const { return iteration_id; }
+
+ void set_cell_size(real_t p_cell_size);
+ real_t get_cell_size() const {
+ return cell_size;
+ }
+
+ void set_merge_rasterizer_cell_scale(float p_value);
+ float get_merge_rasterizer_cell_scale() const {
+ return merge_rasterizer_cell_scale;
+ }
+
+ void set_use_edge_connections(bool p_enabled);
+ bool get_use_edge_connections() const {
+ return use_edge_connections;
+ }
+
+ void set_edge_connection_margin(real_t p_edge_connection_margin);
+ real_t get_edge_connection_margin() const {
+ return edge_connection_margin;
+ }
+
+ void set_link_connection_radius(real_t p_link_connection_radius);
+ real_t get_link_connection_radius() const {
+ return link_connection_radius;
+ }
+
+ nav_2d::PointKey get_point_key(const Vector2 &p_pos) const;
+ Vector2 get_merge_rasterizer_cell_size() const;
+
+ void query_path(NavMeshQueries2D::NavMeshPathQueryTask2D &p_query_task);
+
+ Vector2 get_closest_point(const Vector2 &p_point) const;
+ nav_2d::ClosestPointQueryResult get_closest_point_info(const Vector2 &p_point) const;
+ RID get_closest_point_owner(const Vector2 &p_point) const;
+
+ void add_region(NavRegion2D *p_region);
+ void remove_region(NavRegion2D *p_region);
+ const LocalVector &get_regions() const {
+ return regions;
+ }
+
+ void add_link(NavLink2D *p_link);
+ void remove_link(NavLink2D *p_link);
+ const LocalVector &get_links() const {
+ return links;
+ }
+
+ bool has_agent(NavAgent2D *p_agent) const;
+ void add_agent(NavAgent2D *p_agent);
+ void remove_agent(NavAgent2D *p_agent);
+ const LocalVector &get_agents() const {
+ return agents;
+ }
+
+ void set_agent_as_controlled(NavAgent2D *p_agent);
+ void remove_agent_as_controlled(NavAgent2D *p_agent);
+
+ bool has_obstacle(NavObstacle2D *p_obstacle) const;
+ void add_obstacle(NavObstacle2D *p_obstacle);
+ void remove_obstacle(NavObstacle2D *p_obstacle);
+ const LocalVector &get_obstacles() const {
+ return obstacles;
+ }
+
+ Vector2 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
+
+ void sync();
+ void step(real_t p_deltatime);
+ void dispatch_callbacks();
+
+ // Performance Monitor
+ int get_pm_region_count() const { return performance_data.pm_region_count; }
+ int get_pm_agent_count() const { return performance_data.pm_agent_count; }
+ int get_pm_link_count() const { return performance_data.pm_link_count; }
+ int get_pm_polygon_count() const { return performance_data.pm_polygon_count; }
+ int get_pm_edge_count() const { return performance_data.pm_edge_count; }
+ int get_pm_edge_merge_count() const { return performance_data.pm_edge_merge_count; }
+ int get_pm_edge_connection_count() const { return performance_data.pm_edge_connection_count; }
+ int get_pm_edge_free_count() const { return performance_data.pm_edge_free_count; }
+ int get_pm_obstacle_count() const { return performance_data.pm_obstacle_count; }
+
+ int get_region_connections_count(NavRegion2D *p_region) const;
+ Vector2 get_region_connection_pathway_start(NavRegion2D *p_region, int p_connection_id) const;
+ Vector2 get_region_connection_pathway_end(NavRegion2D *p_region, int p_connection_id) const;
+
+ void add_region_sync_dirty_request(SelfList *p_sync_request);
+ void add_link_sync_dirty_request(SelfList *p_sync_request);
+ void add_agent_sync_dirty_request(SelfList *p_sync_request);
+ void add_obstacle_sync_dirty_request(SelfList *p_sync_request);
+
+ void remove_region_sync_dirty_request(SelfList *p_sync_request);
+ void remove_link_sync_dirty_request(SelfList