diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt
index e5330770d1..4a0dec3815 100644
--- a/COPYRIGHT.txt
+++ b/COPYRIGHT.txt
@@ -332,6 +332,11 @@ Comment: Recast
Copyright: 2009, Mikko Mononen
License: Zlib
+Files: ./thirdparty/rvo2/
+Comment: RVO2
+Copyright: 2016, University of North Carolina at Chapel Hill
+License: Apache 2.0
+
Files: ./thirdparty/squish/
Comment: libSquish
Copyright: 2006, Simon Brown
diff --git a/SConstruct b/SConstruct
index 6af363090b..a8a4acbcdb 100644
--- a/SConstruct
+++ b/SConstruct
@@ -152,6 +152,7 @@ opts.Add(BoolVariable('builtin_opus', "Use the built-in Opus library", True))
opts.Add(BoolVariable('builtin_pcre2', "Use the built-in PCRE2 library", True))
opts.Add(BoolVariable('builtin_pcre2_with_jit', "Use JIT compiler for the built-in PCRE2 library", True))
opts.Add(BoolVariable('builtin_recast', "Use the built-in Recast library", True))
+opts.Add(BoolVariable('builtin_rvo2', "Use the built-in RVO2 library", True))
opts.Add(BoolVariable('builtin_squish', "Use the built-in squish library", True))
opts.Add(BoolVariable('builtin_xatlas', "Use the built-in xatlas library", True))
opts.Add(BoolVariable('builtin_zlib', "Use the built-in zlib library", True))
diff --git a/doc/classes/Navigation2DServer.xml b/doc/classes/Navigation2DServer.xml
new file mode 100644
index 0000000000..105d6aabf8
--- /dev/null
+++ b/doc/classes/Navigation2DServer.xml
@@ -0,0 +1,274 @@
+
+
+
+ Server interface for low-level 2D navigation access
+
+
+ Navigation2DServer is the server responsible for all 2D navigation. It creates the agents, maps, and regions for navigation to work as expected. This keeps tracks of any call and executes them during the sync phase. This means that you can request any change to the map, using any thread, without worrying.
+
+
+
+
+
+
+
+
+ Creates the agent.
+
+
+
+
+
+
+
+
+ Returns true if the map got changed the previous frame.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Callback called at the end of the RVO process.
+
+
+
+
+
+
+
+
+
+
+ Puts the agent in the map.
+
+
+
+
+
+
+
+
+
+
+ Sets the maximum number of other agents the agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+
+
+
+
+
+
+
+
+
+
+ Sets the maximum speed of the agent. Must be positive.
+
+
+
+
+
+
+
+
+
+
+ Sets the maximum distance to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+
+
+
+
+
+
+
+
+
+
+ Sets the position of the agent in world space.
+
+
+
+
+
+
+
+
+
+
+ Sets the radius of the agent.
+
+
+
+
+
+
+
+
+
+
+ Sets the new target velocity.
+
+
+
+
+
+
+
+
+
+
+ The minimal amount of time for which the agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive.
+
+
+
+
+
+
+
+
+
+
+ Sets the current velocity of the agent.
+
+
+
+
+
+
+
+
+ Destroy the RID
+
+
+
+
+
+
+ Create a new map.
+
+
+
+
+
+
+
+
+ Returns the map cell size.
+
+
+
+
+
+
+
+
+ Returns the edge connection margin of the map. The edge connection margin is a distance used to connect two regions.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Returns the navigation path to reach the destination from the origin, while avoiding static obstacles.
+
+
+
+
+
+
+
+
+ Returns true if the map is active.
+
+
+
+
+
+
+
+
+
+
+ Sets the map active.
+
+
+
+
+
+
+
+
+
+
+ Set the map cell size used to weld the navigation mesh polygons.
+
+
+
+
+
+
+
+
+
+
+ Set the map edge connection margin used to weld the compatible region edges.
+
+
+
+
+
+
+ Creates a new region.
+
+
+
+
+
+
+
+
+
+
+ Sets the map for the region.
+
+
+
+
+
+
+
+
+
+
+ Sets the navigation mesh for the region.
+
+
+
+
+
+
+
+
+
+
+ Sets the global transformation for the region.
+
+
+
+
diff --git a/doc/classes/NavigationAgent.xml b/doc/classes/NavigationAgent.xml
new file mode 100644
index 0000000000..d0e2a60fbf
--- /dev/null
+++ b/doc/classes/NavigationAgent.xml
@@ -0,0 +1,163 @@
+
+
+
+ 3D Agent used in navigation for collision avoidance.
+
+
+ 3D Agent that is used in navigation to reach a location while avoiding static and dynamic obstacles. The dynamic obstacles are avoided using RVO collision avoidance. The agent needs navigation data to work correctly. This can be done by having the agent as a child of a [Navigation] node, or using [method set_navigation]. [NavigationAgent] is physics safe.
+
+
+
+
+
+
+
+
+ Returns the distance to the target location, using the agent's global position. The user must set the target location with [method set_target_location] in order for this to be accurate.
+
+
+
+
+
+
+ Returns the reachable final location in global coordinates. This can change if the navigation path is altered in any way. Because of this, it would be best to check this each frame.
+
+
+
+
+
+
+ Returns the path from start to finish in global coordinates.
+
+
+
+
+
+
+ Returns which index the agent is currently on in the navigation path's [PoolVector3Array].
+
+
+
+
+
+
+ Returns the [Navigation] node that the agent is using for its navigation system.
+
+
+
+
+
+
+ Returns a [Vector3] in global coordinates, that can be moved to, making sure that there are no static objects in the way. If the agent does not have a navigation path, it will return the origin of the agent's parent.
+
+
+
+
+
+
+ Returns the user defined [Vector3] after setting the target location.
+
+
+
+
+
+
+ Returns true if the navigation path's final location has been reached.
+
+
+
+
+
+
+ Returns true if the target location is reachable. The target location is set using [method set_target_location].
+
+
+
+
+
+
+ Returns true if the target location is reached. The target location is set using [method set_target_location]. It may not always be possible to reach the target location. It should always be possible to reach the final location though. See [method get_final_location].
+
+
+
+
+
+
+
+
+ Sets the [Navigation] node used by the agent. Useful when you don't want to make the agent a child of a [Navigation] node.
+
+
+
+
+
+
+
+
+ Sets the user desired final location. This will clear the current navigation path.
+
+
+
+
+
+
+
+
+ Sends the passed in velocity to the collision avoidance algorithm. It will adjust the velocity to avoid collisions. Once the adjustment to the velocity is complete, it will emit the [signal velocity_computed] signal.
+
+
+
+
+
+ The distance threshold before a target is considered to be reached. This will allow an agent to not have to hit a point on the path exactly, but in the area.
+
+
+ The radius of the agent.
+
+
+ The agent height offset to match the navigation mesh height.
+
+
+ The distance to search for other agents.
+
+
+ The maximum number of neighbors for the agent to consider.
+
+
+ The minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithim, are safe with respect to other agents. The larger the number, the sooner the agent will respond to other agents, but less freedom in choosing its velocities. Must be positive.
+
+
+ The maximum speed that an agent can move.
+
+
+ The maximum distance the agent is allowed away from the ideal path to the final location. This can happen due to trying to avoid collisions. When the maximum distance is exceded, it recalculates the ideal path.
+
+
+ Ignores collisions on the Y axis. Must be true to move on a horizontal plane.
+
+
+
+
+
+ Notifies when the final location is reached.
+
+
+
+
+ Notifies when the navigation path changes.
+
+
+
+
+ Notifies when the player defined target, set with [method set_target_location], is reached.
+
+
+
+
+
+
+ Notifies when the collision avoidance velocity is calculated. Emitted by [method set_velocity].
+
+
+
+
diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml
new file mode 100644
index 0000000000..dd3a8aa011
--- /dev/null
+++ b/doc/classes/NavigationAgent2D.xml
@@ -0,0 +1,157 @@
+
+
+
+ 2D Agent used in navigation for collision avoidance.
+
+
+ 2D Agent that is used in navigation to reach a location while avoiding static and dynamic obstacles. The dynamic obstacles are avoided using RVO collision avoidance. The agent needs navigation data to work correctly. This can be done by having the agent as a child of a [Navigation2D] node, or using [method set_navigation]. [NavigationAgent2D] is physics safe.
+
+
+
+
+
+
+
+
+ Returns the distance to the target location, using the agent's global position. The user must set the target location with [method set_target_location] in order for this to be accurate.
+
+
+
+
+
+
+ Returns the reachable final location in global coordinates. This can change if the navigation path is altered in any way.
+
+
+
+
+
+
+ Returns the path from start to finish in global coordinates.
+
+
+
+
+
+
+ Returns which index the agent is currently on in the navigation path's [PoolVector2Array].
+
+
+
+
+
+
+ Returns the [Navigation2D] node that the agent is using for its navigation system.
+
+
+
+
+
+
+ Returns a [Vector2] in global coordinates, that can be moved to, making sure that there are no static objects in the way. If the agent does not have a navigation path, it will return the position of the agent's parent.
+
+
+
+
+
+
+ Returns the user defined [Vector2] after setting the target location.
+
+
+
+
+
+
+ Returns true if the navigation path's final location has been reached.
+
+
+
+
+
+
+ Returns true if the target location is reachable. The target location is set using [method set_target_location].
+
+
+
+
+
+
+ Returns true if the target location is reached. The target location is set using [method set_target_location]. It may not always be possible to reach the target location. It should always be possible to reach the final location though. See [method get_final_location].
+
+
+
+
+
+
+
+
+ Sets the [Navigation2D] node used by the agent. Useful when you don't want to make the agent a child of a [Navigation2D] node.
+
+
+
+
+
+
+
+
+ Sets the user desired final location. This will clear the current navigation path.
+
+
+
+
+
+
+
+
+ Sends the passed in velocity to the collision avoidance algorithm. It will adjust the velocity to avoid collisions. Once the adjustment to the velocity is complete, it will emit the [signal velocity_computed] signal.
+
+
+
+
+
+ The distance threshold before a target is considered to be reached. This will allow an agent to not have to hit a point on the path exactly, but in the area.
+
+
+ The radius of the agent.
+
+
+ The distance to search for other agents.
+
+
+ The maximum number of neighbors for the agent to consider.
+
+
+ The minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithim, are safe with respect to other agents. The larger the number, the sooner the agent will respond to other agents, but less freedom in choosing its velocities. Must be positive.
+
+
+ The maximum speed that an agent can move.
+
+
+ The maximum distance the agent is allowed away from the ideal path to the final location. This can happen due to trying to avoid collisions. When the maximum distance is exceded, it recalculates the ideal path.
+
+
+
+
+
+ Notifies when the final location is reached.
+
+
+
+
+ Notifies when the navigation path changes.
+
+
+
+
+ Notifies when the player defined target, set with [method set_target_location], is reached.
+
+
+
+
+
+
+ Notifies when the collision avoidance velocity is calculated. Emitted by [method set_velocity].
+
+
+
+
diff --git a/doc/classes/NavigationMeshInstance.xml b/doc/classes/NavigationMeshInstance.xml
index a348740fdc..75bd62e278 100644
--- a/doc/classes/NavigationMeshInstance.xml
+++ b/doc/classes/NavigationMeshInstance.xml
@@ -1,19 +1,42 @@
+ An instance of a [NavigationMesh].
+ An instance of a [NavigationMesh]. It tells the [Navigation] node what can be navigated and what cannot, based on the [NavigationMesh] resource. This should be a child of a [Navigation] node.
+
+
+
+
+ Bakes the [NavigationMesh]. The baking is done in a seperate thread because navigation baking is not a cheap operation. This can be done at runtime. When it is completed, it automatically sets the new [NavigationMesh].
+
+
+ Determines if the [NavigationMeshInstance] is enabled or disabled.
+ The [NavigationMesh] resource to use.
+
+
+
+ Notifies when the navigation mesh bake operation is completed.
+
+
+
+
+ Notifies when the [NavigationMesh] has changed.
+
+
+
diff --git a/doc/classes/NavigationObstacle.xml b/doc/classes/NavigationObstacle.xml
new file mode 100644
index 0000000000..307538b77b
--- /dev/null
+++ b/doc/classes/NavigationObstacle.xml
@@ -0,0 +1,29 @@
+
+
+
+ 3D Obstacle used in navigation for collision avoidance.
+
+
+ 3D Obstacle used in navigation for collision avoidance. The obstacle needs navigation data to work correctly. This can be done by having the obstacle as a child of a [Navigation] node, or using [method set_navigation]. [NavigationObstacle] is physics safe.
+
+
+
+
+
+
+
+
+ Returns the [Navigation] node that the obstacle is using for its navigation system.
+
+
+
+
+
+
+
+
+ Sets the [Navigation] node used by the obstacle. Useful when you don't want to make the obstacle a child of a [Navigation] node.
+
+
+
+
diff --git a/doc/classes/NavigationObstacle2D.xml b/doc/classes/NavigationObstacle2D.xml
new file mode 100644
index 0000000000..cf5dd2fa10
--- /dev/null
+++ b/doc/classes/NavigationObstacle2D.xml
@@ -0,0 +1,29 @@
+
+
+
+ 2D Obstacle used in navigation for collision avoidance.
+
+
+ 2D Obstacle used in navigation for collision avoidance. The obstacle needs navigation data to work correctly. This can be done by having the obstacle as a child of a [Navigation2D] node, or using [method set_navigation]. [NavigationObstacle] is physics safe.
+
+
+
+
+
+
+
+
+ Returns the [Navigation2D] node that the obstacle is using for its navigation system.
+
+
+
+
+
+
+
+
+ Sets the [Navigation2D] node used by the obstacle. Useful when you don't want to make the obstacle a child of a [Navigation2D] node.
+
+
+
+
diff --git a/doc/classes/NavigationServer.xml b/doc/classes/NavigationServer.xml
new file mode 100644
index 0000000000..894a46b8a7
--- /dev/null
+++ b/doc/classes/NavigationServer.xml
@@ -0,0 +1,323 @@
+
+
+
+ Server interface for low-level 3D navigation access
+
+
+ NavigationServer is the server responsible for all 3D navigation. It creates the agents, maps, and regions for navigation to work as expected. This keeps tracks of any call and executes them during the sync phase. This means that you can request any change to the map, using any thread, without worrying.
+
+
+
+
+
+
+
+
+ Creates the agent.
+
+
+
+
+
+
+
+
+ Returns true if the map got changed the previous frame.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Callback called at the end of the RVO process.
+
+
+
+
+
+
+
+
+
+
+ Puts the agent in the map.
+
+
+
+
+
+
+
+
+
+
+ Sets the maximum number of other agents the agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+
+
+
+
+
+
+
+
+
+
+ Sets the maximum speed of the agent. Must be positive.
+
+
+
+
+
+
+
+
+
+
+ Sets the maximum distance to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
+
+
+
+
+
+
+
+
+
+
+ Sets the position of the agent in world space.
+
+
+
+
+
+
+
+
+
+
+ Sets the radius of the agent.
+
+
+
+
+
+
+
+
+
+
+ Sets the new target velocity.
+
+
+
+
+
+
+
+
+
+
+ The minimal amount of time for which the agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive.
+
+
+
+
+
+
+
+
+
+
+ Sets the current velocity of the agent.
+
+
+
+
+
+
+
+
+ Destroy the RID
+
+
+
+
+
+
+ Create a new map.
+
+
+
+
+
+
+
+
+ Returns the map cell size.
+
+
+
+
+
+
+
+
+ Returns the edge connection margin of the map.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Returns the navigation path to reach the destination from the origin.
+
+
+
+
+
+
+
+
+ Returns the map's up direction.
+
+
+
+
+
+
+
+
+ Returns true if the map is active.
+
+
+
+
+
+
+
+
+
+
+ Sets the map active.
+
+
+
+
+
+
+
+
+
+
+ Set the map cell size used to weld the navigation mesh polygons.
+
+
+
+
+
+
+
+
+
+
+ Set the map edge connection margein used to weld the compatible region edges.
+
+
+
+
+
+
+
+
+
+
+ Sets the map up direction.
+
+
+
+
+
+
+
+
+
+
+ Bakes the navigation mesh.
+
+
+
+
+
+
+ Creates a new region.
+
+
+
+
+
+
+
+
+
+
+ Sets the map for the region.
+
+
+
+
+
+
+
+
+
+
+ Sets the navigation mesh for the region.
+
+
+
+
+
+
+
+
+
+
+ Sets the global transformation for the region.
+
+
+
+
+
+
+
+
+ Control activation of this server.
+
+
+
+
+
+
+
+
+ Steps the server. This is not threadsafe and must be called in single thread.
+
+
+
+
diff --git a/editor/editor_node.cpp b/editor/editor_node.cpp
index 7d0601e8db..f7d8d2c7c4 100644
--- a/editor/editor_node.cpp
+++ b/editor/editor_node.cpp
@@ -62,6 +62,8 @@
#include "scene/gui/texture_progress.h"
#include "scene/gui/tool_button.h"
#include "scene/resources/packed_scene.h"
+#include "servers/navigation_2d_server.h"
+#include "servers/navigation_server.h"
#include "servers/physics_2d_server.h"
#include "editor/audio_stream_preview.h"
@@ -5584,6 +5586,8 @@ EditorNode::EditorNode() {
VisualServer::get_singleton()->textures_keep_original(true);
VisualServer::get_singleton()->set_debug_generate_wireframes(true);
+ NavigationServer::get_singleton()->set_active(false); // no nav by default if editor
+
PhysicsServer::get_singleton()->set_active(false); // no physics by default if editor
Physics2DServer::get_singleton()->set_active(false); // no physics by default if editor
ScriptServer::set_scripting_enabled(false); // no scripting by default if editor
diff --git a/editor/plugins/mesh_instance_editor_plugin.cpp b/editor/plugins/mesh_instance_editor_plugin.cpp
index 6e5307cebe..aaba6406c7 100644
--- a/editor/plugins/mesh_instance_editor_plugin.cpp
+++ b/editor/plugins/mesh_instance_editor_plugin.cpp
@@ -32,7 +32,7 @@
#include "editor/editor_scale.h"
#include "scene/3d/collision_shape.h"
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/physics_body.h"
#include "scene/gui/box_container.h"
#include "spatial_editor_plugin.h"
diff --git a/editor/plugins/mesh_library_editor_plugin.cpp b/editor/plugins/mesh_library_editor_plugin.cpp
index 92898ff9c3..42859e9190 100644
--- a/editor/plugins/mesh_library_editor_plugin.cpp
+++ b/editor/plugins/mesh_library_editor_plugin.cpp
@@ -34,7 +34,7 @@
#include "editor/editor_settings.h"
#include "main/main.h"
#include "scene/3d/mesh_instance.h"
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/physics_body.h"
#include "scene/main/viewport.h"
#include "scene/resources/packed_scene.h"
diff --git a/editor/spatial_editor_gizmos.cpp b/editor/spatial_editor_gizmos.cpp
index c94b0eeab0..d012b2f821 100644
--- a/editor/spatial_editor_gizmos.cpp
+++ b/editor/spatial_editor_gizmos.cpp
@@ -41,7 +41,7 @@
#include "scene/3d/light.h"
#include "scene/3d/listener.h"
#include "scene/3d/mesh_instance.h"
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/particles.h"
#include "scene/3d/physics_joint.h"
#include "scene/3d/position_3d.h"
diff --git a/main/main.cpp b/main/main.cpp
index 650a680d87..162d5568d4 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -66,6 +66,8 @@
#include "servers/arvr_server.h"
#include "servers/audio_server.h"
#include "servers/camera_server.h"
+#include "servers/navigation_2d_server.h"
+#include "servers/navigation_server.h"
#include "servers/physics_2d_server.h"
#include "servers/physics_server.h"
#include "servers/register_server_types.h"
@@ -103,6 +105,8 @@ static CameraServer *camera_server = NULL;
static ARVRServer *arvr_server = NULL;
static PhysicsServer *physics_server = NULL;
static Physics2DServer *physics_2d_server = NULL;
+static NavigationServer *navigation_server = NULL;
+static Navigation2DServer *navigation_2d_server = NULL;
// We error out if setup2() doesn't turn this true
static bool _start_success = false;
@@ -197,6 +201,19 @@ void finalize_physics() {
memdelete(physics_2d_server);
}
+void initialize_navigation_server() {
+ ERR_FAIL_COND(navigation_server != NULL);
+ navigation_server = NavigationServerManager::new_default_server();
+ navigation_2d_server = memnew(Navigation2DServer);
+}
+
+void finalize_navigation_server() {
+ memdelete(navigation_server);
+ navigation_server = NULL;
+ memdelete(navigation_2d_server);
+ navigation_2d_server = NULL;
+}
+
//#define DEBUG_INIT
#ifdef DEBUG_INIT
#define MAIN_PRINT(m_txt) print_line(m_txt)
@@ -1356,6 +1373,7 @@ Error Main::setup2(Thread::ID p_main_tid_override) {
camera_server = CameraServer::create();
initialize_physics();
+ initialize_navigation_server();
register_server_singletons();
register_driver_types();
@@ -2011,6 +2029,8 @@ bool Main::iteration() {
message_queue->flush();
+ NavigationServer::get_singleton_mut()->step(frame_slice * time_scale);
+
PhysicsServer::get_singleton()->step(frame_slice * time_scale);
Physics2DServer::get_singleton()->end_sync();
@@ -2202,6 +2222,7 @@ void Main::cleanup() {
OS::get_singleton()->finalize();
finalize_physics();
+ finalize_navigation_server();
if (packed_data)
memdelete(packed_data);
diff --git a/modules/gdnavigation/SCsub b/modules/gdnavigation/SCsub
new file mode 100644
index 0000000000..9d462f92a7
--- /dev/null
+++ b/modules/gdnavigation/SCsub
@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+
+Import('env')
+Import('env_modules')
+
+env_navigation = env_modules.Clone()
+
+# Recast Thirdparty source files
+if env['builtin_recast']:
+ thirdparty_dir = "#thirdparty/recastnavigation/Recast/"
+ thirdparty_sources = [
+ "Source/Recast.cpp",
+ "Source/RecastAlloc.cpp",
+ "Source/RecastArea.cpp",
+ "Source/RecastAssert.cpp",
+ "Source/RecastContour.cpp",
+ "Source/RecastFilter.cpp",
+ "Source/RecastLayers.cpp",
+ "Source/RecastMesh.cpp",
+ "Source/RecastMeshDetail.cpp",
+ "Source/RecastRasterization.cpp",
+ "Source/RecastRegion.cpp",
+ ]
+ thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+ env_navigation.Prepend(CPPPATH=[thirdparty_dir + "/Include"])
+
+ env_thirdparty = env_navigation.Clone()
+ env_thirdparty.disable_warnings()
+ env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources)
+
+
+# RVO Thirdparty source files
+if env['builtin_rvo2']:
+ thirdparty_dir = "#thirdparty/rvo2"
+ thirdparty_sources = [
+ "/src/Agent.cpp",
+ "/src/KdTree.cpp",
+ ]
+ thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+ env_navigation.Prepend(CPPPATH=[thirdparty_dir + "/src"])
+
+ env_thirdparty = env_navigation.Clone()
+ env_thirdparty.disable_warnings()
+ env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources)
+
+
+# Godot source files
+env_navigation.add_source_files(env.modules_sources, "*.cpp")
diff --git a/modules/recast/config.py b/modules/gdnavigation/config.py
similarity index 71%
rename from modules/recast/config.py
rename to modules/gdnavigation/config.py
index 098f1eafa9..1c8cd12a2d 100644
--- a/modules/recast/config.py
+++ b/modules/gdnavigation/config.py
@@ -1,5 +1,5 @@
def can_build(env, platform):
- return env['tools']
+ return True
def configure(env):
pass
diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
new file mode 100644
index 0000000000..f3ffd93c9c
--- /dev/null
+++ b/modules/gdnavigation/gd_navigation_server.cpp
@@ -0,0 +1,472 @@
+/*************************************************************************/
+/* gd_navigation_server.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* 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 "gd_navigation_server.h"
+
+#include "core/os/mutex.h"
+
+#ifndef _3D_DISABLED
+#include "navigation_mesh_generator.h"
+#endif
+
+/**
+ @author AndreaCatania
+*/
+
+/// Creates a struct for each function and a function that once called creates
+/// an instance of that struct with the submited parameters.
+/// Then, that struct is stored in an array; the `sync` function consume that array.
+
+#define COMMAND_1(F_NAME, T_0, D_0) \
+ struct MERGE(F_NAME, _command) : public SetCommand { \
+ T_0 d_0; \
+ MERGE(F_NAME, _command) \
+ (T_0 p_d_0) : \
+ d_0(p_d_0) {} \
+ virtual void exec(GdNavigationServer *server) { \
+ server->MERGE(_cmd_, F_NAME)(d_0); \
+ } \
+ }; \
+ void GdNavigationServer::F_NAME(T_0 D_0) const { \
+ auto cmd = memnew(MERGE(F_NAME, _command)( \
+ D_0)); \
+ add_command(cmd); \
+ } \
+ void GdNavigationServer::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) : public SetCommand { \
+ T_0 d_0; \
+ T_1 d_1; \
+ MERGE(F_NAME, _command) \
+ ( \
+ T_0 p_d_0, \
+ T_1 p_d_1) : \
+ d_0(p_d_0), \
+ d_1(p_d_1) {} \
+ virtual void exec(GdNavigationServer *server) { \
+ server->MERGE(_cmd_, F_NAME)(d_0, d_1); \
+ } \
+ }; \
+ void GdNavigationServer::F_NAME(T_0 D_0, T_1 D_1) const { \
+ auto cmd = memnew(MERGE(F_NAME, _command)( \
+ D_0, \
+ D_1)); \
+ add_command(cmd); \
+ } \
+ void GdNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+#define COMMAND_4(F_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3) \
+ struct MERGE(F_NAME, _command) : public SetCommand { \
+ T_0 d_0; \
+ T_1 d_1; \
+ T_2 d_2; \
+ T_3 d_3; \
+ MERGE(F_NAME, _command) \
+ ( \
+ T_0 p_d_0, \
+ T_1 p_d_1, \
+ T_2 p_d_2, \
+ T_3 p_d_3) : \
+ d_0(p_d_0), \
+ d_1(p_d_1), \
+ d_2(p_d_2), \
+ d_3(p_d_3) {} \
+ virtual void exec(GdNavigationServer *server) { \
+ server->MERGE(_cmd_, F_NAME)(d_0, d_1, d_2, d_3); \
+ } \
+ }; \
+ void GdNavigationServer::F_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) const { \
+ auto cmd = memnew(MERGE(F_NAME, _command)( \
+ D_0, \
+ D_1, \
+ D_2, \
+ D_3)); \
+ add_command(cmd); \
+ } \
+ void GdNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3)
+
+GdNavigationServer::GdNavigationServer() :
+ NavigationServer(),
+ active(true) {
+ commands_mutex = Mutex::create();
+ operations_mutex = Mutex::create();
+}
+
+GdNavigationServer::~GdNavigationServer() {}
+
+void GdNavigationServer::add_command(SetCommand *command) const {
+ auto mut_this = const_cast(this);
+ commands_mutex->lock();
+ mut_this->commands.push_back(command);
+ commands_mutex->unlock();
+}
+
+RID GdNavigationServer::map_create() const {
+ auto mut_this = const_cast(this);
+ mut_this->operations_mutex->lock();
+ NavMap *space = memnew(NavMap);
+ RID rid = map_owner.make_rid(space);
+ space->set_self(rid);
+ mut_this->operations_mutex->unlock();
+ return rid;
+}
+
+COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND(map == NULL);
+
+ if (p_active) {
+ if (!map_is_active(p_map)) {
+ active_maps.push_back(map);
+ }
+ } else {
+ active_maps.erase(map);
+ }
+}
+
+bool GdNavigationServer::map_is_active(RID p_map) const {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND_V(map == NULL, false);
+
+ return active_maps.find(map) >= 0;
+}
+
+COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND(map == NULL);
+
+ map->set_up(p_up);
+}
+
+Vector3 GdNavigationServer::map_get_up(RID p_map) const {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND_V(map == NULL, Vector3());
+
+ return map->get_up();
+}
+
+COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND(map == NULL);
+
+ map->set_cell_size(p_cell_size);
+}
+
+real_t GdNavigationServer::map_get_cell_size(RID p_map) const {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND_V(map == NULL, 0);
+
+ return map->get_cell_size();
+}
+
+COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin) {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND(map == NULL);
+
+ map->set_edge_connection_margin(p_connection_margin);
+}
+
+real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND_V(map == NULL, 0);
+
+ return map->get_edge_connection_margin();
+}
+
+Vector GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND_V(map == NULL, Vector());
+
+ return map->get_path(p_origin, p_destination, p_optimize);
+}
+
+RID GdNavigationServer::region_create() const {
+ auto mut_this = const_cast(this);
+ mut_this->operations_mutex->lock();
+ NavRegion *reg = memnew(NavRegion);
+ RID rid = region_owner.make_rid(reg);
+ reg->set_self(rid);
+ mut_this->operations_mutex->unlock();
+ return rid;
+}
+
+COMMAND_2(region_set_map, RID, p_region, RID, p_map) {
+ NavRegion *region = region_owner.get(p_region);
+ ERR_FAIL_COND(region == NULL);
+
+ if (region->get_map() != NULL) {
+
+ if (region->get_map()->get_self() == p_map)
+ return; // Pointless
+
+ region->get_map()->remove_region(region);
+ region->set_map(NULL);
+ }
+
+ if (p_map.is_valid()) {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND(map == NULL);
+
+ map->add_region(region);
+ region->set_map(map);
+ }
+}
+
+COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform) {
+ NavRegion *region = region_owner.get(p_region);
+ ERR_FAIL_COND(region == NULL);
+
+ region->set_transform(p_transform);
+}
+
+COMMAND_2(region_set_navmesh, RID, p_region, Ref, p_nav_mesh) {
+ NavRegion *region = region_owner.get(p_region);
+ ERR_FAIL_COND(region == NULL);
+
+ region->set_mesh(p_nav_mesh);
+}
+
+void GdNavigationServer::region_bake_navmesh(Ref r_mesh, Node *p_node) const {
+ ERR_FAIL_COND(r_mesh.is_null());
+ ERR_FAIL_COND(p_node == NULL);
+
+#ifndef _3D_DISABLED
+ NavigationMeshGenerator::get_singleton()->clear(r_mesh);
+ NavigationMeshGenerator::get_singleton()->bake(r_mesh, p_node);
+#endif
+}
+
+RID GdNavigationServer::agent_create() const {
+ auto mut_this = const_cast(this);
+ mut_this->operations_mutex->lock();
+ RvoAgent *agent = memnew(RvoAgent());
+ RID rid = agent_owner.make_rid(agent);
+ agent->set_self(rid);
+ mut_this->operations_mutex->unlock();
+ return rid;
+}
+
+COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ if (agent->get_map()) {
+ if (agent->get_map()->get_self() == p_map)
+ return; // Pointless
+
+ agent->get_map()->remove_agent(agent);
+ }
+
+ agent->set_map(NULL);
+
+ if (p_map.is_valid()) {
+ NavMap *map = map_owner.get(p_map);
+ ERR_FAIL_COND(map == NULL);
+
+ agent->set_map(map);
+ map->add_agent(agent);
+
+ if (agent->has_callback()) {
+ map->set_agent_as_controlled(agent);
+ }
+ }
+}
+
+COMMAND_2(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->neighborDist_ = p_dist;
+}
+
+COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->maxNeighbors_ = p_count;
+}
+
+COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->timeHorizon_ = p_time;
+}
+
+COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->radius_ = p_radius;
+}
+
+COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->maxSpeed_ = p_max_speed;
+}
+
+COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->velocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+}
+
+COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->prefVelocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+}
+
+COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->position_ = RVO::Vector3(p_position.x, p_position.y, p_position.z);
+}
+
+COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->get_agent()->ignore_y_ = p_ignore;
+}
+
+bool GdNavigationServer::agent_is_map_changed(RID p_agent) const {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND_V(agent == NULL, false);
+
+ return agent->is_map_changed();
+}
+
+COMMAND_4(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata) {
+ RvoAgent *agent = agent_owner.get(p_agent);
+ ERR_FAIL_COND(agent == NULL);
+
+ agent->set_callback(p_receiver == NULL ? 0 : p_receiver->get_instance_id(), p_method, p_udata);
+
+ if (agent->get_map()) {
+ if (p_receiver == NULL) {
+ agent->get_map()->remove_agent_as_controlled(agent);
+ } else {
+ agent->get_map()->set_agent_as_controlled(agent);
+ }
+ }
+}
+
+COMMAND_1(free, RID, p_object) {
+ if (map_owner.owns(p_object)) {
+ NavMap *map = map_owner.get(p_object);
+
+ // Removes any assigned region
+ std::vector regions = map->get_regions();
+ for (size_t i(0); i < regions.size(); i++) {
+ map->remove_region(regions[i]);
+ regions[i]->set_map(NULL);
+ }
+
+ // Remove any assigned agent
+ std::vector agents = map->get_agents();
+ for (size_t i(0); i < agents.size(); i++) {
+ map->remove_agent(agents[i]);
+ agents[i]->set_map(NULL);
+ }
+
+ active_maps.erase(map);
+ map_owner.free(p_object);
+ memdelete(map);
+
+ } else if (region_owner.owns(p_object)) {
+ NavRegion *region = region_owner.get(p_object);
+
+ // Removes this region from the map if assigned
+ if (region->get_map() != NULL) {
+ region->get_map()->remove_region(region);
+ region->set_map(NULL);
+ }
+
+ region_owner.free(p_object);
+ memdelete(region);
+
+ } else if (agent_owner.owns(p_object)) {
+ RvoAgent *agent = agent_owner.get(p_object);
+
+ // Removes this agent from the map if assigned
+ if (agent->get_map() != NULL) {
+ agent->get_map()->remove_agent(agent);
+ agent->set_map(NULL);
+ }
+
+ agent_owner.free(p_object);
+ memdelete(agent);
+
+ } else {
+ ERR_FAIL_COND("Invalid ID.");
+ }
+}
+
+void GdNavigationServer::set_active(bool p_active) const {
+ auto mut_this = const_cast(this);
+ mut_this->operations_mutex->lock();
+ mut_this->active = p_active;
+ mut_this->operations_mutex->unlock();
+}
+
+void GdNavigationServer::step(real_t p_delta_time) {
+ if (!active) {
+ return;
+ }
+
+ // With c++ we can't be 100% sure this is called in single thread so use the mutex.
+ commands_mutex->lock();
+ operations_mutex->lock();
+ for (size_t i(0); i < commands.size(); i++) {
+ commands[i]->exec(this);
+ memdelete(commands[i]);
+ }
+ commands.clear();
+ operations_mutex->unlock();
+ commands_mutex->unlock();
+
+ // These are internal operations so don't need to be shielded.
+ for (int i(0); i < active_maps.size(); i++) {
+ active_maps[i]->sync();
+ active_maps[i]->step(p_delta_time);
+ active_maps[i]->dispatch_callbacks();
+ }
+}
+
+#undef COMMAND_1
+#undef COMMAND_2
+#undef COMMAND_4
diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h
new file mode 100644
index 0000000000..80ba06880c
--- /dev/null
+++ b/modules/gdnavigation/gd_navigation_server.h
@@ -0,0 +1,134 @@
+/*************************************************************************/
+/* gd_navigation_server.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef GD_NAVIGATION_SERVER_H
+#define GD_NAVIGATION_SERVER_H
+
+#include "servers/navigation_server.h"
+
+#include "nav_map.h"
+#include "nav_region.h"
+#include "rvo_agent.h"
+
+/**
+ @author AndreaCatania
+*/
+
+/// 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) const; \
+ 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) const; \
+ void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+#define COMMAND_4_DEF(F_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, D_3_DEF) \
+ virtual void F_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3 = D_3_DEF) const; \
+ void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3)
+
+class GdNavigationServer;
+class Mutex;
+
+struct SetCommand {
+ virtual ~SetCommand() {}
+ virtual void exec(GdNavigationServer *server) = 0;
+};
+
+class GdNavigationServer : public NavigationServer {
+ Mutex *commands_mutex;
+ /// Mutex used to make any operation threadsafe.
+ Mutex *operations_mutex;
+
+ std::vector commands;
+
+ mutable RID_Owner map_owner;
+ mutable RID_Owner region_owner;
+ mutable RID_Owner agent_owner;
+
+ bool active;
+ Vector active_maps;
+
+public:
+ GdNavigationServer();
+ virtual ~GdNavigationServer();
+
+ void add_command(SetCommand *command) const;
+
+ virtual RID map_create() const;
+ COMMAND_2(map_set_active, RID, p_map, bool, p_active);
+ virtual bool map_is_active(RID p_map) const;
+
+ COMMAND_2(map_set_up, RID, p_map, Vector3, p_up);
+ virtual Vector3 map_get_up(RID p_map) const;
+
+ 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;
+
+ 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;
+
+ virtual Vector map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const;
+
+ virtual RID region_create() const;
+ COMMAND_2(region_set_map, RID, p_region, RID, p_map);
+ COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform);
+ COMMAND_2(region_set_navmesh, RID, p_region, Ref, p_nav_mesh);
+ virtual void region_bake_navmesh(Ref r_mesh, Node *p_node) const;
+
+ virtual RID agent_create() const;
+ COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
+ COMMAND_2(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist);
+ COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
+ COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time);
+ COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
+ COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
+ COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity);
+ COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity);
+ COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position);
+ COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore);
+ virtual bool agent_is_map_changed(RID p_agent) const;
+ COMMAND_4_DEF(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata, Variant());
+
+ COMMAND_1(free, RID, p_object);
+
+ virtual void set_active(bool p_active) const;
+ virtual void step(real_t p_delta_time);
+};
+
+#undef COMMAND_1
+#undef COMMAND_2
+#undef COMMAND_4_DEF
+
+#endif // GD_NAVIGATION_SERVER_H
diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp
new file mode 100644
index 0000000000..3d0e02c77d
--- /dev/null
+++ b/modules/gdnavigation/nav_map.cpp
@@ -0,0 +1,665 @@
+/*************************************************************************/
+/* rvo_space.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* 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.h"
+
+#include "core/os/threaded_array_processor.h"
+#include "nav_region.h"
+#include "rvo_agent.h"
+#include
+
+/**
+ @author AndreaCatania
+*/
+
+#define USE_ENTRY_POINT
+
+NavMap::NavMap() :
+ up(0, 1, 0),
+ cell_size(0.3),
+ edge_connection_margin(5.0),
+ regenerate_polygons(true),
+ regenerate_links(true),
+ agents_dirty(false),
+ deltatime(0.0),
+ map_update_id(0) {}
+
+void NavMap::set_up(Vector3 p_up) {
+ up = p_up;
+ regenerate_polygons = true;
+}
+
+void NavMap::set_cell_size(float p_cell_size) {
+ cell_size = p_cell_size;
+ regenerate_polygons = true;
+}
+
+void NavMap::set_edge_connection_margin(float p_edge_connection_margin) {
+ edge_connection_margin = p_edge_connection_margin;
+ regenerate_links = true;
+}
+
+gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
+ const int x = int(Math::floor(p_pos.x / cell_size));
+ const int y = int(Math::floor(p_pos.y / cell_size));
+ const int z = int(Math::floor(p_pos.z / cell_size));
+
+ gd::PointKey p;
+ p.key = 0;
+ p.x = x;
+ p.y = y;
+ p.z = z;
+ return p;
+}
+
+Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
+
+ const gd::Polygon *begin_poly = NULL;
+ const gd::Polygon *end_poly = NULL;
+ Vector3 begin_point;
+ Vector3 end_point;
+ float begin_d = 1e20;
+ float end_d = 1e20;
+
+ // Find the initial poly and the end poly on this map.
+ for (size_t i(0); i < polygons.size(); i++) {
+ const gd::Polygon &p = polygons[i];
+
+ // For each point cast a face and check the distance between the origin/destination
+ for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+
+ Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ Vector3 spoint = f.get_closest_point_to(p_origin);
+ float dpoint = spoint.distance_to(p_origin);
+ if (dpoint < begin_d) {
+ begin_d = dpoint;
+ begin_poly = &p;
+ begin_point = spoint;
+ }
+
+ spoint = f.get_closest_point_to(p_destination);
+ dpoint = spoint.distance_to(p_destination);
+ if (dpoint < end_d) {
+ end_d = dpoint;
+ end_poly = &p;
+ end_point = spoint;
+ }
+ }
+ }
+
+ if (!begin_poly || !end_poly) {
+ // No path
+ return Vector();
+ }
+
+ if (begin_poly == end_poly) {
+
+ Vector path;
+ path.resize(2);
+ path.write[0] = begin_point;
+ path.write[1] = end_point;
+ return path;
+ }
+
+ std::vector navigation_polys;
+ navigation_polys.reserve(polygons.size() * 0.75);
+
+ // The elements indices in the `navigation_polys`.
+ int least_cost_id(-1);
+ List open_list;
+ bool found_route = false;
+
+ navigation_polys.push_back(gd::NavigationPoly(begin_poly));
+ {
+ least_cost_id = 0;
+ gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+ least_cost_poly->self_id = least_cost_id;
+ least_cost_poly->entry = begin_point;
+ }
+
+ open_list.push_back(0);
+
+ const gd::Polygon *reachable_end = NULL;
+ float reachable_d = 1e30;
+ bool is_reachable = true;
+
+ while (found_route == false) {
+
+ {
+ // Takes the current least_cost_poly neighbors and compute the traveled_distance of each
+ for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
+ gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+
+ const gd::Edge &edge = least_cost_poly->poly->edges[i];
+ if (!edge.other_polygon)
+ continue;
+
+#ifdef USE_ENTRY_POINT
+ Vector3 edge_line[2] = {
+ least_cost_poly->poly->points[i].pos,
+ least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos
+ };
+
+ const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly->entry, edge_line);
+ const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance;
+#else
+ const float new_distance = least_cost_poly->poly->center.distance_to(edge.other_polygon->center) + least_cost_poly->traveled_distance;
+#endif
+
+ auto it = std::find(
+ navigation_polys.begin(),
+ navigation_polys.end(),
+ gd::NavigationPoly(edge.other_polygon));
+
+ if (it != navigation_polys.end()) {
+ // Oh this was visited already, can we win the cost?
+ if (it->traveled_distance > new_distance) {
+
+ it->prev_navigation_poly_id = least_cost_id;
+ it->back_navigation_edge = edge.other_edge;
+ it->traveled_distance = new_distance;
+#ifdef USE_ENTRY_POINT
+ it->entry = new_entry;
+#endif
+ }
+ } else {
+ // Add to open neighbours
+
+ navigation_polys.push_back(gd::NavigationPoly(edge.other_polygon));
+ gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1];
+
+ np->self_id = navigation_polys.size() - 1;
+ np->prev_navigation_poly_id = least_cost_id;
+ np->back_navigation_edge = edge.other_edge;
+ np->traveled_distance = new_distance;
+#ifdef USE_ENTRY_POINT
+ np->entry = new_entry;
+#endif
+ open_list.push_back(navigation_polys.size() - 1);
+ }
+ }
+ }
+
+ // Removes the least cost polygon from the open list so we can advance.
+ open_list.erase(least_cost_id);
+
+ if (open_list.size() == 0) {
+ // When the open list is empty at this point the End Polygon is not reachable
+ // so 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 == NULL) {
+ // 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;
+ end_d = 1e20;
+ for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
+ Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+ Vector3 spoint = f.get_closest_point_to(p_destination);
+ float dpoint = spoint.distance_to(p_destination);
+ if (dpoint < end_d) {
+ end_point = spoint;
+ end_d = dpoint;
+ }
+ }
+
+ // Reset open and navigation_polys
+ gd::NavigationPoly np = navigation_polys[0];
+ navigation_polys.clear();
+ navigation_polys.push_back(np);
+ open_list.clear();
+ open_list.push_back(0);
+
+ reachable_end = NULL;
+
+ continue;
+ }
+
+ // Now take the new least_cost_poly from the open list.
+ least_cost_id = -1;
+ float least_cost = 1e30;
+
+ for (auto element = open_list.front(); element != NULL; element = element->next()) {
+ gd::NavigationPoly *np = &navigation_polys[element->get()];
+ float cost = np->traveled_distance;
+#ifdef USE_ENTRY_POINT
+ cost += np->entry.distance_to(end_point);
+#else
+ cost += np->poly->center.distance_to(end_point);
+#endif
+ if (cost < least_cost) {
+ least_cost_id = np->self_id;
+ least_cost = cost;
+ }
+ }
+
+ // Stores the further reachable end polygon, in case our goal is not reachable.
+ if (is_reachable) {
+ float d = navigation_polys[least_cost_id].entry.distance_to(p_destination);
+ if (reachable_d > d) {
+ reachable_d = d;
+ reachable_end = navigation_polys[least_cost_id].poly;
+ }
+ }
+
+ ERR_BREAK(least_cost_id == -1);
+
+ // Check if we reached the end
+ if (navigation_polys[least_cost_id].poly == end_poly) {
+ // Yep, done!!
+ found_route = true;
+ break;
+ }
+ }
+
+ if (found_route) {
+
+ Vector path;
+ if (p_optimize) {
+
+ // String pulling
+
+ gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+ Vector3 apex_point = end_point;
+ Vector3 portal_left = apex_point;
+ Vector3 portal_right = apex_point;
+ gd::NavigationPoly *left_poly = apex_poly;
+ gd::NavigationPoly *right_poly = apex_poly;
+ gd::NavigationPoly *p = apex_poly;
+
+ path.push_back(end_point);
+
+ while (p) {
+
+ Vector3 left;
+ Vector3 right;
+
+#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b)))
+
+ if (p->poly == begin_poly) {
+ left = begin_point;
+ right = begin_point;
+ } else {
+ int prev = p->back_navigation_edge;
+ int prev_n = (p->back_navigation_edge + 1) % p->poly->points.size();
+ left = p->poly->points[prev].pos;
+ right = p->poly->points[prev_n].pos;
+
+ //if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){
+ if (p->poly->clockwise) {
+ SWAP(left, right);
+ }
+ }
+
+ bool skip = false;
+
+ if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) {
+ //process
+ if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) {
+ left_poly = p;
+ portal_left = left;
+ } else {
+
+ clip_path(navigation_polys, path, apex_poly, portal_right, right_poly);
+
+ apex_point = portal_right;
+ p = right_poly;
+ left_poly = p;
+ apex_poly = p;
+ portal_left = apex_point;
+ portal_right = apex_point;
+ path.push_back(apex_point);
+ skip = true;
+ }
+ }
+
+ if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) {
+ //process
+ if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) {
+ right_poly = p;
+ portal_right = right;
+ } else {
+
+ clip_path(navigation_polys, path, apex_poly, portal_left, left_poly);
+
+ apex_point = portal_left;
+ p = left_poly;
+ right_poly = p;
+ apex_poly = p;
+ portal_right = apex_point;
+ portal_left = apex_point;
+ path.push_back(apex_point);
+ }
+ }
+
+ if (p->prev_navigation_poly_id != -1)
+ p = &navigation_polys[p->prev_navigation_poly_id];
+ else
+ // The end
+ p = NULL;
+ }
+
+ if (path[path.size() - 1] != begin_point)
+ path.push_back(begin_point);
+
+ path.invert();
+
+ } else {
+ path.push_back(end_point);
+
+ // Add mid points
+ int np_id = least_cost_id;
+ while (np_id != -1) {
+
+#ifdef USE_ENTRY_POINT
+ Vector3 point = navigation_polys[np_id].entry;
+#else
+ 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();
+ Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
+#endif
+
+ path.push_back(point);
+ np_id = navigation_polys[np_id].prev_navigation_poly_id;
+ }
+
+ path.invert();
+ }
+
+ return path;
+ }
+ return Vector();
+}
+
+void NavMap::add_region(NavRegion *p_region) {
+ regions.push_back(p_region);
+ regenerate_links = true;
+}
+
+void NavMap::remove_region(NavRegion *p_region) {
+ regions.push_back(p_region);
+ regenerate_links = true;
+}
+
+bool NavMap::has_agent(RvoAgent *agent) const {
+ return std::find(agents.begin(), agents.end(), agent) != agents.end();
+}
+
+void NavMap::add_agent(RvoAgent *agent) {
+ if (!has_agent(agent)) {
+ agents.push_back(agent);
+ agents_dirty = true;
+ }
+}
+
+void NavMap::remove_agent(RvoAgent *agent) {
+ remove_agent_as_controlled(agent);
+ auto it = std::find(agents.begin(), agents.end(), agent);
+ if (it != agents.end()) {
+ agents.erase(it);
+ agents_dirty = true;
+ }
+}
+
+void NavMap::set_agent_as_controlled(RvoAgent *agent) {
+ const bool exist = std::find(controlled_agents.begin(), controlled_agents.end(), agent) != controlled_agents.end();
+ if (!exist) {
+ ERR_FAIL_COND(!has_agent(agent));
+ controlled_agents.push_back(agent);
+ }
+}
+
+void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
+ auto it = std::find(controlled_agents.begin(), controlled_agents.end(), agent);
+ if (it != controlled_agents.end()) {
+ controlled_agents.erase(it);
+ }
+}
+
+void NavMap::sync() {
+
+ if (regenerate_polygons) {
+ for (size_t r(0); r < regions.size(); r++) {
+ regions[r]->scratch_polygons();
+ }
+ regenerate_links = true;
+ }
+
+ for (size_t r(0); r < regions.size(); r++) {
+ if (regions[r]->sync()) {
+ regenerate_links = true;
+ }
+ }
+
+ if (regenerate_links) {
+ // Copy all region polygons in the map.
+ int count = 0;
+ for (size_t r(0); r < regions.size(); r++) {
+ count += regions[r]->get_polygons().size();
+ }
+
+ polygons.resize(count);
+ count = 0;
+
+ for (size_t r(0); r < regions.size(); r++) {
+ std::copy(
+ regions[r]->get_polygons().data(),
+ regions[r]->get_polygons().data() + regions[r]->get_polygons().size(),
+ polygons.begin() + count);
+
+ count += regions[r]->get_polygons().size();
+ }
+
+ // Connects the `Edges` of all the `Polygons` of all `Regions` each other.
+ Map connections;
+
+ for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) {
+ gd::Polygon &poly(polygons[poly_id]);
+
+ for (size_t p(0); p < poly.points.size(); p++) {
+ int next_point = (p + 1) % poly.points.size();
+ gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
+
+ Map::Element *connection = connections.find(ek);
+ if (!connection) {
+ // Nothing yet
+ gd::Connection c;
+ c.A = &poly;
+ c.A_edge = p;
+ c.B = NULL;
+ c.B_edge = -1;
+ connections[ek] = c;
+
+ } else if (connection->get().B == NULL) {
+ CRASH_COND(connection->get().A == NULL); // Unreachable
+
+ // Connect the two Polygons by this edge
+ connection->get().B = &poly;
+ connection->get().B_edge = p;
+
+ connection->get().A->edges[connection->get().A_edge].this_edge = connection->get().A_edge;
+ connection->get().A->edges[connection->get().A_edge].other_polygon = connection->get().B;
+ connection->get().A->edges[connection->get().A_edge].other_edge = connection->get().B_edge;
+
+ connection->get().B->edges[connection->get().B_edge].this_edge = connection->get().B_edge;
+ connection->get().B->edges[connection->get().B_edge].other_polygon = connection->get().A;
+ connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge;
+ } else {
+ // The edge is already connected with another edge, skip.
+ }
+ }
+ }
+
+ // Takes all the free edges.
+ std::vector free_edges;
+ free_edges.reserve(connections.size());
+
+ for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) {
+ if (connection_element->get().B == NULL) {
+ CRASH_COND(connection_element->get().A == NULL); // Unreachable
+ CRASH_COND(connection_element->get().A_edge < 0); // Unreachable
+
+ // This is a free edge
+ uint32_t id(free_edges.size());
+ free_edges.push_back(gd::FreeEdge());
+ free_edges[id].is_free = true;
+ free_edges[id].poly = connection_element->get().A;
+ free_edges[id].edge_id = connection_element->get().A_edge;
+ uint32_t point_0(free_edges[id].edge_id);
+ uint32_t point_1((free_edges[id].edge_id + 1) % free_edges[id].poly->points.size());
+ Vector3 pos_0 = free_edges[id].poly->points[point_0].pos;
+ Vector3 pos_1 = free_edges[id].poly->points[point_1].pos;
+ Vector3 relative = pos_1 - pos_0;
+ free_edges[id].edge_center = (pos_0 + pos_1) / 2.0;
+ free_edges[id].edge_dir = relative.normalized();
+ free_edges[id].edge_len_squared = relative.length_squared();
+ }
+ }
+
+ const float ecm_squared(edge_connection_margin * edge_connection_margin);
+#define LEN_TOLLERANCE 0.1
+#define DIR_TOLLERANCE 0.9
+ // In front of tollerance
+#define IFO_TOLLERANCE 0.5
+
+ // 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.
+ for (size_t i(0); i < free_edges.size(); i++) {
+ if (!free_edges[i].is_free) {
+ continue;
+ }
+ gd::FreeEdge &edge = free_edges[i];
+ for (size_t y(0); y < free_edges.size(); y++) {
+ gd::FreeEdge &other_edge = free_edges[y];
+ if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) {
+ continue;
+ }
+
+ Vector3 rel_centers = other_edge.edge_center - edge.edge_center;
+ if (ecm_squared > rel_centers.length_squared() // Are enough closer?
+ && ABS(edge.edge_len_squared - other_edge.edge_len_squared) < LEN_TOLLERANCE // Are the same length?
+ && ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLLERANCE // Are alligned?
+ && ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLLERANCE // Are one in front the other?
+ ) {
+ // The edges can be connected
+ edge.is_free = false;
+ other_edge.is_free = false;
+
+ edge.poly->edges[edge.edge_id].this_edge = edge.edge_id;
+ edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id;
+ edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly;
+
+ other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id;
+ other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id;
+ other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly;
+ }
+ }
+ }
+ }
+
+ if (regenerate_links) {
+ map_update_id = map_update_id + 1 % 9999999;
+ }
+
+ if (agents_dirty) {
+ std::vector raw_agents;
+ raw_agents.reserve(agents.size());
+ for (size_t i(0); i < agents.size(); i++)
+ raw_agents.push_back(agents[i]->get_agent());
+ rvo.buildAgentTree(raw_agents);
+ }
+
+ regenerate_polygons = false;
+ regenerate_links = false;
+ agents_dirty = false;
+}
+
+void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) {
+ (*(agent + index))->get_agent()->computeNeighbors(&rvo);
+ (*(agent + index))->get_agent()->computeNewVelocity(deltatime);
+}
+
+void NavMap::step(real_t p_deltatime) {
+ deltatime = p_deltatime;
+ if (controlled_agents.size() > 0) {
+ thread_process_array(
+ controlled_agents.size(),
+ this,
+ &NavMap::compute_single_step,
+ controlled_agents.data());
+ }
+}
+
+void NavMap::dispatch_callbacks() {
+ for (int i(0); i < static_cast(controlled_agents.size()); i++) {
+ controlled_agents[i]->dispatch_callback();
+ }
+}
+
+void NavMap::clip_path(const std::vector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const {
+ Vector3 from = path[path.size() - 1];
+
+ if (from.distance_to(p_to_point) < CMP_EPSILON)
+ return;
+ Plane cut_plane;
+ cut_plane.normal = (from - p_to_point).cross(up);
+ if (cut_plane.normal == Vector3())
+ return;
+ cut_plane.normal.normalize();
+ cut_plane.d = cut_plane.normal.dot(from);
+
+ while (from_poly != p_to_poly) {
+
+ int back_nav_edge = from_poly->back_navigation_edge;
+ Vector3 a = from_poly->poly->points[back_nav_edge].pos;
+ Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos;
+
+ ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1);
+ from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id];
+
+ if (a.distance_to(b) > CMP_EPSILON) {
+
+ Vector3 inters;
+ if (cut_plane.intersects_segment(a, b, &inters)) {
+ if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
+ path.push_back(inters);
+ }
+ }
+ }
+ }
+}
diff --git a/modules/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h
new file mode 100644
index 0000000000..636c07e437
--- /dev/null
+++ b/modules/gdnavigation/nav_map.h
@@ -0,0 +1,137 @@
+/*************************************************************************/
+/* rvo_space.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef RVO_SPACE_H
+#define RVO_SPACE_H
+
+#include "nav_rid.h"
+
+#include "core/math/math_defs.h"
+#include "nav_utils.h"
+#include
+
+/**
+ @author AndreaCatania
+*/
+
+class NavRegion;
+class RvoAgent;
+class NavRegion;
+
+class NavMap : public NavRid {
+
+ /// Map Up
+ Vector3 up;
+
+ /// To find the polygons edges the vertices are displaced in a grid where
+ /// each cell has the following cell_size.
+ real_t cell_size;
+
+ /// This value is used to detect the near edges to connect.
+ real_t edge_connection_margin;
+
+ bool regenerate_polygons;
+ bool regenerate_links;
+
+ std::vector regions;
+
+ /// Map polygons
+ std::vector polygons;
+
+ /// Rvo world
+ RVO::KdTree rvo;
+
+ /// Is agent array modified?
+ bool agents_dirty;
+
+ /// All the Agents (even the controlled one)
+ std::vector agents;
+
+ /// Controlled agents
+ std::vector controlled_agents;
+
+ /// Physics delta time
+ real_t deltatime;
+
+ /// Change the id each time the map is updated.
+ uint32_t map_update_id;
+
+public:
+ NavMap();
+
+ void set_up(Vector3 p_up);
+ Vector3 get_up() const {
+ return up;
+ }
+
+ void set_cell_size(float p_cell_size);
+ float get_cell_size() const {
+ return cell_size;
+ }
+
+ void set_edge_connection_margin(float p_edge_connection_margin);
+ float get_edge_connection_margin() const {
+ return edge_connection_margin;
+ }
+
+ gd::PointKey get_point_key(const Vector3 &p_pos) const;
+
+ Vector get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const;
+
+ void add_region(NavRegion *p_region);
+ void remove_region(NavRegion *p_region);
+ const std::vector &get_regions() const {
+ return regions;
+ }
+
+ bool has_agent(RvoAgent *agent) const;
+ void add_agent(RvoAgent *agent);
+ void remove_agent(RvoAgent *agent);
+ const std::vector &get_agents() const {
+ return agents;
+ }
+
+ void set_agent_as_controlled(RvoAgent *agent);
+ void remove_agent_as_controlled(RvoAgent *agent);
+
+ uint32_t get_map_update_id() const {
+ return map_update_id;
+ }
+
+ void sync();
+ void step(real_t p_deltatime);
+ void dispatch_callbacks();
+
+private:
+ void compute_single_step(uint32_t index, RvoAgent **agent);
+ void clip_path(const std::vector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const;
+};
+
+#endif // RVO_SPACE_H
diff --git a/modules/gdnavigation/nav_region.cpp b/modules/gdnavigation/nav_region.cpp
new file mode 100644
index 0000000000..03477085ce
--- /dev/null
+++ b/modules/gdnavigation/nav_region.cpp
@@ -0,0 +1,136 @@
+/*************************************************************************/
+/* nav_region.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "nav_region.h"
+
+#include "nav_map.h"
+
+/**
+ @author AndreaCatania
+*/
+
+NavRegion::NavRegion() :
+ map(NULL),
+ polygons_dirty(true) {
+}
+
+void NavRegion::set_map(NavMap *p_map) {
+ map = p_map;
+ polygons_dirty = true;
+}
+
+void NavRegion::set_transform(Transform p_transform) {
+ transform = p_transform;
+ polygons_dirty = true;
+}
+
+void NavRegion::set_mesh(Ref p_mesh) {
+ mesh = p_mesh;
+ polygons_dirty = true;
+}
+
+bool NavRegion::sync() {
+ bool something_changed = polygons_dirty /* || something_dirty? */;
+
+ update_polygons();
+
+ return something_changed;
+}
+
+void NavRegion::update_polygons() {
+ if (!polygons_dirty) {
+ return;
+ }
+ polygons.clear();
+ polygons_dirty = false;
+
+ if (map == NULL) {
+ return;
+ }
+
+ if (mesh.is_null())
+ return;
+
+ PoolVector vertices = mesh->get_vertices();
+ int len = vertices.size();
+ if (len == 0)
+ return;
+
+ PoolVector::Read vertices_r = vertices.read();
+
+ polygons.resize(mesh->get_polygon_count());
+
+ // Build
+ for (size_t i(0); i < polygons.size(); i++) {
+
+ gd::Polygon &p = polygons[i];
+ p.owner = this;
+
+ Vector mesh_poly = mesh->get_polygon(i);
+ const int *indices = mesh_poly.ptr();
+ bool valid(true);
+ p.points.resize(mesh_poly.size());
+ p.edges.resize(mesh_poly.size());
+
+ Vector3 center;
+ float sum(0);
+
+ for (int j(0); j < mesh_poly.size(); j++) {
+
+ int idx = indices[j];
+ if (idx < 0 || idx >= len) {
+ valid = false;
+ break;
+ }
+
+ Vector3 point_position = transform.xform(vertices_r[idx]);
+ p.points[j].pos = point_position;
+ p.points[j].key = map->get_point_key(point_position);
+
+ center += point_position; // Composing the center of the polygon
+
+ if (j >= 2) {
+ Vector3 epa = transform.xform(vertices_r[indices[j - 2]]);
+ Vector3 epb = transform.xform(vertices_r[indices[j - 1]]);
+
+ sum += map->get_up().dot((epb - epa).cross(point_position - epa));
+ }
+ }
+
+ if (!valid) {
+ ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!");
+ }
+
+ p.clockwise = sum > 0;
+ if (mesh_poly.size() != 0) {
+ p.center = center / float(mesh_poly.size());
+ }
+ }
+}
diff --git a/modules/gdnavigation/nav_region.h b/modules/gdnavigation/nav_region.h
new file mode 100644
index 0000000000..c961257163
--- /dev/null
+++ b/modules/gdnavigation/nav_region.h
@@ -0,0 +1,89 @@
+/*************************************************************************/
+/* nav_region.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAV_REGION_H
+#define NAV_REGION_H
+
+#include "nav_rid.h"
+
+#include "nav_utils.h"
+#include "scene/3d/navigation.h"
+#include
+
+/**
+ @author AndreaCatania
+*/
+
+class NavMap;
+class NavRegion;
+
+class NavRegion : public NavRid {
+ NavMap *map;
+ Transform transform;
+ Ref mesh;
+
+ bool polygons_dirty;
+
+ /// Cache
+ std::vector polygons;
+
+public:
+ NavRegion();
+
+ void scratch_polygons() {
+ polygons_dirty = true;
+ }
+
+ void set_map(NavMap *p_map);
+ NavMap *get_map() const {
+ return map;
+ }
+
+ void set_transform(Transform transform);
+ const Transform &get_transform() const {
+ return transform;
+ }
+
+ void set_mesh(Ref p_mesh);
+ const Ref get_mesh() const {
+ return mesh;
+ }
+
+ std::vector const &get_polygons() const {
+ return polygons;
+ }
+
+ bool sync();
+
+private:
+ void update_polygons();
+};
+
+#endif // NAV_REGION_H
diff --git a/modules/gdnavigation/nav_rid.h b/modules/gdnavigation/nav_rid.h
new file mode 100644
index 0000000000..96e22800c2
--- /dev/null
+++ b/modules/gdnavigation/nav_rid.h
@@ -0,0 +1,48 @@
+/*************************************************************************/
+/* rvo_rid.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAV_RID_H
+#define NAV_RID_H
+
+#include "core/rid.h"
+
+/**
+ @author AndreaCatania
+*/
+
+class NavRid : public RID_Data {
+ RID self;
+
+public:
+ _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+ _FORCE_INLINE_ RID get_self() const { return self; }
+};
+
+#endif // NAV_RID_H
diff --git a/modules/gdnavigation/nav_utils.h b/modules/gdnavigation/nav_utils.h
new file mode 100644
index 0000000000..1048f58294
--- /dev/null
+++ b/modules/gdnavigation/nav_utils.h
@@ -0,0 +1,169 @@
+/*************************************************************************/
+/* utils.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAV_UTILS_H
+#define NAV_UTILS_H
+
+#include "core/math/vector3.h"
+#include
+
+/**
+ @author AndreaCatania
+*/
+
+class NavRegion;
+
+namespace gd {
+struct Polygon;
+
+union PointKey {
+
+ struct {
+ int64_t x : 21;
+ int64_t y : 22;
+ int64_t z : 21;
+ };
+
+ uint64_t key;
+ bool operator<(const PointKey &p_key) const { return key < p_key.key; }
+};
+
+struct EdgeKey {
+
+ PointKey a;
+ PointKey b;
+
+ bool operator<(const EdgeKey &p_key) const {
+ return (a.key == p_key.a.key) ? (b.key < p_key.b.key) : (a.key < p_key.a.key);
+ }
+
+ EdgeKey(const PointKey &p_a = PointKey(), const PointKey &p_b = PointKey()) :
+ a(p_a),
+ b(p_b) {
+ if (a.key > b.key) {
+ SWAP(a, b);
+ }
+ }
+};
+
+struct Point {
+ Vector3 pos;
+ PointKey key;
+};
+
+struct Edge {
+ /// This edge ID
+ int this_edge;
+
+ /// Other Polygon
+ Polygon *other_polygon;
+
+ /// The other `Polygon` at this edge id has this `Polygon`.
+ int other_edge;
+
+ Edge() {
+ this_edge = -1;
+ other_polygon = NULL;
+ other_edge = -1;
+ }
+};
+
+struct Polygon {
+ NavRegion *owner;
+
+ /// The points of this `Polygon`
+ std::vector points;
+
+ /// Are the points clockwise ?
+ bool clockwise;
+
+ /// The edges of this `Polygon`
+ std::vector edges;
+
+ /// The center of this `Polygon`
+ Vector3 center;
+};
+
+struct Connection {
+
+ Polygon *A;
+ int A_edge;
+ Polygon *B;
+ int B_edge;
+
+ Connection() {
+ A = NULL;
+ B = NULL;
+ A_edge = -1;
+ B_edge = -1;
+ }
+};
+
+struct NavigationPoly {
+ uint32_t self_id;
+ /// This poly.
+ const Polygon *poly;
+ /// The previous navigation poly (id in the `navigation_poly` array).
+ int prev_navigation_poly_id;
+ /// The edge id in this `Poly` to reach the `prev_navigation_poly_id`.
+ uint32_t back_navigation_edge;
+ /// The entry location of this poly.
+ Vector3 entry;
+ /// The distance to the destination.
+ float traveled_distance;
+
+ NavigationPoly(const Polygon *p_poly) :
+ self_id(0),
+ poly(p_poly),
+ prev_navigation_poly_id(-1),
+ back_navigation_edge(0),
+ traveled_distance(0.0) {
+ }
+
+ bool operator==(const NavigationPoly &other) const {
+ return this->poly == other.poly;
+ }
+
+ bool operator!=(const NavigationPoly &other) const {
+ return !operator==(other);
+ }
+};
+
+struct FreeEdge {
+ bool is_free;
+ Polygon *poly;
+ uint32_t edge_id;
+ Vector3 edge_center;
+ Vector3 edge_dir;
+ float edge_len_squared;
+};
+} // namespace gd
+
+#endif // NAV_UTILS_H
diff --git a/modules/recast/navigation_mesh_editor_plugin.cpp b/modules/gdnavigation/navigation_mesh_editor_plugin.cpp
similarity index 91%
rename from modules/recast/navigation_mesh_editor_plugin.cpp
rename to modules/gdnavigation/navigation_mesh_editor_plugin.cpp
index 6e68dba8ee..13c74d5706 100644
--- a/modules/recast/navigation_mesh_editor_plugin.cpp
+++ b/modules/gdnavigation/navigation_mesh_editor_plugin.cpp
@@ -28,10 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
+#ifdef TOOLS_ENABLED
#include "navigation_mesh_editor_plugin.h"
#include "core/io/marshalls.h"
#include "core/io/resource_saver.h"
+#include "navigation_mesh_generator.h"
#include "scene/3d/mesh_instance.h"
#include "scene/gui/box_container.h"
@@ -57,15 +59,14 @@ void NavigationMeshEditor::_bake_pressed() {
button_bake->set_pressed(false);
ERR_FAIL_COND(!node);
- const String conf_warning = node->get_configuration_warning();
- if (!conf_warning.empty()) {
- err_dialog->set_text(conf_warning);
+ if (!node->get_navigation_mesh().is_valid()) {
+ err_dialog->set_text(TTR("A NavigationMesh resource must be set or created for this node to work."));
err_dialog->popup_centered_minsize();
return;
}
- EditorNavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
- EditorNavigationMeshGenerator::get_singleton()->bake(node->get_navigation_mesh(), node);
+ NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
+ NavigationMeshGenerator::get_singleton()->bake(node->get_navigation_mesh(), node);
node->update_gizmo();
}
@@ -73,7 +74,7 @@ void NavigationMeshEditor::_bake_pressed() {
void NavigationMeshEditor::_clear_pressed() {
if (node)
- EditorNavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
+ NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
button_bake->set_pressed(false);
bake_info->set_text("");
@@ -160,3 +161,5 @@ NavigationMeshEditorPlugin::NavigationMeshEditorPlugin(EditorNode *p_node) {
NavigationMeshEditorPlugin::~NavigationMeshEditorPlugin() {
}
+
+#endif
diff --git a/modules/recast/navigation_mesh_editor_plugin.h b/modules/gdnavigation/navigation_mesh_editor_plugin.h
similarity index 95%
rename from modules/recast/navigation_mesh_editor_plugin.h
rename to modules/gdnavigation/navigation_mesh_editor_plugin.h
index 09c8673b43..f5833f3d7f 100644
--- a/modules/recast/navigation_mesh_editor_plugin.h
+++ b/modules/gdnavigation/navigation_mesh_editor_plugin.h
@@ -28,12 +28,15 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
-#ifndef NAVIGATION_MESH_GENERATOR_PLUGIN_H
-#define NAVIGATION_MESH_GENERATOR_PLUGIN_H
+#ifndef NAVIGATION_MESH_EDITOR_PLUGIN_H
+#define NAVIGATION_MESH_EDITOR_PLUGIN_H
+
+#ifdef TOOLS_ENABLED
#include "editor/editor_node.h"
#include "editor/editor_plugin.h"
-#include "navigation_mesh_generator.h"
+
+class NavigationMeshInstance;
class NavigationMeshEditor : public Control {
friend class NavigationMeshEditorPlugin;
@@ -81,4 +84,6 @@ public:
~NavigationMeshEditorPlugin();
};
-#endif // NAVIGATION_MESH_GENERATOR_PLUGIN_H
+#endif
+
+#endif
diff --git a/modules/recast/navigation_mesh_generator.cpp b/modules/gdnavigation/navigation_mesh_generator.cpp
similarity index 84%
rename from modules/recast/navigation_mesh_generator.cpp
rename to modules/gdnavigation/navigation_mesh_generator.cpp
index b6f5b38038..04b86fabc5 100644
--- a/modules/recast/navigation_mesh_generator.cpp
+++ b/modules/gdnavigation/navigation_mesh_generator.cpp
@@ -28,11 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
+#ifndef _3D_DISABLED
+
#include "navigation_mesh_generator.h"
#include "core/math/quick_hull.h"
#include "core/os/thread.h"
-#include "editor/editor_settings.h"
#include "scene/3d/collision_shape.h"
#include "scene/3d/mesh_instance.h"
#include "scene/3d/physics_body.h"
@@ -47,6 +48,11 @@
#include "scene/resources/sphere_shape.h"
#include "modules/modules_enabled.gen.h"
+#ifdef TOOLS_ENABLED
+#include "editor/editor_node.h"
+#include "editor/editor_settings.h"
+#endif
+
#ifdef MODULE_CSG_ENABLED
#include "modules/csg/csg_shape.h"
#endif
@@ -54,15 +60,15 @@
#include "modules/gridmap/grid_map.h"
#endif
-EditorNavigationMeshGenerator *EditorNavigationMeshGenerator::singleton = NULL;
+NavigationMeshGenerator *NavigationMeshGenerator::singleton = NULL;
-void EditorNavigationMeshGenerator::_add_vertex(const Vector3 &p_vec3, Vector &p_verticies) {
+void NavigationMeshGenerator::_add_vertex(const Vector3 &p_vec3, Vector &p_verticies) {
p_verticies.push_back(p_vec3.x);
p_verticies.push_back(p_vec3.y);
p_verticies.push_back(p_vec3.z);
}
-void EditorNavigationMeshGenerator::_add_mesh(const Ref &p_mesh, const Transform &p_xform, Vector &p_verticies, Vector &p_indices) {
+void NavigationMeshGenerator::_add_mesh(const Ref &p_mesh, const Transform &p_xform, Vector &p_verticies, Vector &p_indices) {
int current_vertex_count;
for (int i = 0; i < p_mesh->get_surface_count(); i++) {
@@ -117,7 +123,7 @@ void EditorNavigationMeshGenerator::_add_mesh(const Ref &p_mesh, const Tra
}
}
-void EditorNavigationMeshGenerator::_add_faces(const PoolVector3Array &p_faces, const Transform &p_xform, Vector &p_verticies, Vector &p_indices) {
+void NavigationMeshGenerator::_add_faces(const PoolVector3Array &p_faces, const Transform &p_xform, Vector &p_verticies, Vector &p_indices) {
int face_count = p_faces.size() / 3;
int current_vertex_count = p_verticies.size() / 3;
@@ -132,7 +138,7 @@ void EditorNavigationMeshGenerator::_add_faces(const PoolVector3Array &p_faces,
}
}
-void EditorNavigationMeshGenerator::_parse_geometry(Transform p_accumulated_transform, Node *p_node, Vector &p_verticies, Vector &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) {
+void NavigationMeshGenerator::_parse_geometry(Transform p_accumulated_transform, Node *p_node, Vector &p_verticies, Vector &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) {
if (Object::cast_to(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
@@ -271,7 +277,7 @@ void EditorNavigationMeshGenerator::_parse_geometry(Transform p_accumulated_tran
}
}
-void EditorNavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref p_nav_mesh) {
+void NavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref p_nav_mesh) {
PoolVector nav_vertices;
@@ -299,11 +305,24 @@ void EditorNavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_me
}
}
-void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref p_nav_mesh, EditorProgress *ep,
- rcHeightfield *hf, rcCompactHeightfield *chf, rcContourSet *cset, rcPolyMesh *poly_mesh, rcPolyMeshDetail *detail_mesh,
- Vector &vertices, Vector &indices) {
+void NavigationMeshGenerator::_build_recast_navigation_mesh(
+ Ref p_nav_mesh,
+#ifdef TOOLS_ENABLED
+ EditorProgress *ep,
+#endif
+ rcHeightfield *hf,
+ rcCompactHeightfield *chf,
+ rcContourSet *cset,
+ rcPolyMesh *poly_mesh,
+ rcPolyMeshDetail *detail_mesh,
+ Vector &vertices,
+ Vector &indices) {
rcContext ctx;
- ep->step(TTR("Setting up Configuration..."), 1);
+
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Setting up Configuration..."), 1);
+#endif
const float *verts = vertices.ptr();
const int nverts = vertices.size() / 3;
@@ -337,16 +356,25 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Refstep(TTR("Calculating grid size..."), 2);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Calculating grid size..."), 2);
+#endif
rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
- ep->step(TTR("Creating heightfield..."), 3);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Creating heightfield..."), 3);
+#endif
hf = rcAllocHeightfield();
ERR_FAIL_COND(!hf);
ERR_FAIL_COND(!rcCreateHeightfield(&ctx, *hf, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch));
- ep->step(TTR("Marking walkable triangles..."), 4);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Marking walkable triangles..."), 4);
+#endif
{
Vector tri_areas;
tri_areas.resize(ntris);
@@ -366,7 +394,10 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Refget_filter_walkable_low_height_spans())
rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
- ep->step(TTR("Constructing compact heightfield..."), 5);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Constructing compact heightfield..."), 5);
+#endif
chf = rcAllocCompactHeightfield();
@@ -376,10 +407,18 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Refstep(TTR("Eroding walkable area..."), 6);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Eroding walkable area..."), 6);
+#endif
+
ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
- ep->step(TTR("Partitioning..."), 7);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Partitioning..."), 7);
+#endif
+
if (p_nav_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_WATERSHED) {
ERR_FAIL_COND(!rcBuildDistanceField(&ctx, *chf));
ERR_FAIL_COND(!rcBuildRegions(&ctx, *chf, 0, cfg.minRegionArea, cfg.mergeRegionArea));
@@ -389,14 +428,20 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Refstep(TTR("Creating contours..."), 8);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Creating contours..."), 8);
+#endif
cset = rcAllocContourSet();
ERR_FAIL_COND(!cset);
ERR_FAIL_COND(!rcBuildContours(&ctx, *chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset));
- ep->step(TTR("Creating polymesh..."), 9);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Creating polymesh..."), 9);
+#endif
poly_mesh = rcAllocPolyMesh();
ERR_FAIL_COND(!poly_mesh);
@@ -411,7 +456,10 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Refstep(TTR("Converting to native navigation mesh..."), 10);
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Converting to native navigation mesh..."), 10);
+#endif
_convert_detail_mesh_to_native_navigation_mesh(detail_mesh, p_nav_mesh);
@@ -421,23 +469,30 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref p_nav_mesh, Node *p_node) {
+void NavigationMeshGenerator::bake(Ref p_nav_mesh, Node *p_node) {
ERR_FAIL_COND(!p_nav_mesh.is_valid());
- EditorProgress ep("bake", TTR("Navigation Mesh Generator Setup:"), 11);
- ep.step(TTR("Parsing Geometry..."), 0);
+#ifdef TOOLS_ENABLED
+ EditorProgress *ep(NULL);
+ if (Engine::get_singleton()->is_editor_hint()) {
+ ep = memnew(EditorProgress("bake", TTR("Navigation Mesh Generator Setup:"), 11));
+ }
+
+ if (ep)
+ ep->step(TTR("Parsing Geometry..."), 0);
+#endif
Vector vertices;
Vector indices;
@@ -466,7 +521,18 @@ void EditorNavigationMeshGenerator::bake(Ref p_nav_mesh, Node *p
rcPolyMesh *poly_mesh = NULL;
rcPolyMeshDetail *detail_mesh = NULL;
- _build_recast_navigation_mesh(p_nav_mesh, &ep, hf, chf, cset, poly_mesh, detail_mesh, vertices, indices);
+ _build_recast_navigation_mesh(
+ p_nav_mesh,
+#ifdef TOOLS_ENABLED
+ ep,
+#endif
+ hf,
+ chf,
+ cset,
+ poly_mesh,
+ detail_mesh,
+ vertices,
+ indices);
rcFreeHeightField(hf);
hf = 0;
@@ -483,17 +549,26 @@ void EditorNavigationMeshGenerator::bake(Ref p_nav_mesh, Node *p
rcFreePolyMeshDetail(detail_mesh);
detail_mesh = 0;
}
- ep.step(TTR("Done!"), 11);
+
+#ifdef TOOLS_ENABLED
+ if (ep)
+ ep->step(TTR("Done!"), 11);
+
+ if (ep)
+ memdelete(ep);
+#endif
}
-void EditorNavigationMeshGenerator::clear(Ref p_nav_mesh) {
+void NavigationMeshGenerator::clear(Ref p_nav_mesh) {
if (p_nav_mesh.is_valid()) {
p_nav_mesh->clear_polygons();
p_nav_mesh->set_vertices(PoolVector());
}
}
-void EditorNavigationMeshGenerator::_bind_methods() {
- ClassDB::bind_method(D_METHOD("bake", "nav_mesh", "root_node"), &EditorNavigationMeshGenerator::bake);
- ClassDB::bind_method(D_METHOD("clear", "nav_mesh"), &EditorNavigationMeshGenerator::clear);
+void NavigationMeshGenerator::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("bake", "nav_mesh", "root_node"), &NavigationMeshGenerator::bake);
+ ClassDB::bind_method(D_METHOD("clear", "nav_mesh"), &NavigationMeshGenerator::clear);
}
+
+#endif
diff --git a/modules/recast/navigation_mesh_generator.h b/modules/gdnavigation/navigation_mesh_generator.h
similarity index 82%
rename from modules/recast/navigation_mesh_generator.h
rename to modules/gdnavigation/navigation_mesh_generator.h
index 8c7ca8b62c..107dee75e2 100644
--- a/modules/recast/navigation_mesh_generator.h
+++ b/modules/gdnavigation/navigation_mesh_generator.h
@@ -31,15 +31,20 @@
#ifndef NAVIGATION_MESH_GENERATOR_H
#define NAVIGATION_MESH_GENERATOR_H
-#include "editor/editor_node.h"
-#include "scene/3d/navigation_mesh.h"
+#ifndef _3D_DISABLED
+
+#include "scene/3d/navigation_mesh_instance.h"
#include
-class EditorNavigationMeshGenerator : public Object {
- GDCLASS(EditorNavigationMeshGenerator, Object);
+#ifdef TOOLS_ENABLED
+struct EditorProgress;
+#endif
- static EditorNavigationMeshGenerator *singleton;
+class NavigationMeshGenerator : public Object {
+ GDCLASS(NavigationMeshGenerator, Object);
+
+ static NavigationMeshGenerator *singleton;
protected:
static void _bind_methods();
@@ -50,18 +55,29 @@ protected:
static void _parse_geometry(Transform p_accumulated_transform, Node *p_node, Vector &p_verticies, Vector &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children);
static void _convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref p_nav_mesh);
- static void _build_recast_navigation_mesh(Ref p_nav_mesh, EditorProgress *ep,
- rcHeightfield *hf, rcCompactHeightfield *chf, rcContourSet *cset, rcPolyMesh *poly_mesh,
- rcPolyMeshDetail *detail_mesh, Vector &vertices, Vector &indices);
+ static void _build_recast_navigation_mesh(
+ Ref p_nav_mesh,
+#ifdef TOOLS_ENABLED
+ EditorProgress *ep,
+#endif
+ rcHeightfield *hf,
+ rcCompactHeightfield *chf,
+ rcContourSet *cset,
+ rcPolyMesh *poly_mesh,
+ rcPolyMeshDetail *detail_mesh,
+ Vector &vertices,
+ Vector &indices);
public:
- static EditorNavigationMeshGenerator *get_singleton();
+ static NavigationMeshGenerator *get_singleton();
- EditorNavigationMeshGenerator();
- ~EditorNavigationMeshGenerator();
+ NavigationMeshGenerator();
+ ~NavigationMeshGenerator();
void bake(Ref p_nav_mesh, Node *p_node);
void clear(Ref p_nav_mesh);
};
+#endif
+
#endif // NAVIGATION_MESH_GENERATOR_H
diff --git a/modules/recast/register_types.cpp b/modules/gdnavigation/register_types.cpp
similarity index 79%
rename from modules/recast/register_types.cpp
rename to modules/gdnavigation/register_types.cpp
index ea0ab00771..d717733787 100644
--- a/modules/recast/register_types.cpp
+++ b/modules/gdnavigation/register_types.cpp
@@ -30,30 +30,51 @@
#include "register_types.h"
-#include "navigation_mesh_editor_plugin.h"
+#include "core/engine.h"
+#include "gd_navigation_server.h"
+#include "servers/navigation_server.h"
-#ifdef TOOLS_ENABLED
-EditorNavigationMeshGenerator *_nav_mesh_generator = NULL;
+#ifndef _3D_DISABLED
+#include "navigation_mesh_generator.h"
#endif
-void register_recast_types() {
#ifdef TOOLS_ENABLED
+#include "navigation_mesh_editor_plugin.h"
+#endif
+
+/**
+ @author AndreaCatania
+*/
+
+#ifndef _3D_DISABLED
+NavigationMeshGenerator *_nav_mesh_generator = NULL;
+#endif
+
+NavigationServer *new_server() {
+ return memnew(GdNavigationServer);
+}
+
+void register_gdnavigation_types() {
+ NavigationServerManager::set_default_server(new_server);
+
+#ifndef _3D_DISABLED
+ _nav_mesh_generator = memnew(NavigationMeshGenerator);
+ ClassDB::register_class();
+ Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton()));
+#endif
+
+#ifdef TOOLS_ENABLED
+ EditorPlugins::add_by_type();
+
ClassDB::APIType prev_api = ClassDB::get_current_api();
ClassDB::set_current_api(ClassDB::API_EDITOR);
- EditorPlugins::add_by_type();
- _nav_mesh_generator = memnew(EditorNavigationMeshGenerator);
-
- ClassDB::register_class();
-
- Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", EditorNavigationMeshGenerator::get_singleton()));
-
ClassDB::set_current_api(prev_api);
#endif
}
-void unregister_recast_types() {
-#ifdef TOOLS_ENABLED
+void unregister_gdnavigation_types() {
+#ifndef _3D_DISABLED
if (_nav_mesh_generator) {
memdelete(_nav_mesh_generator);
}
diff --git a/modules/recast/register_types.h b/modules/gdnavigation/register_types.h
similarity index 95%
rename from modules/recast/register_types.h
rename to modules/gdnavigation/register_types.h
index d16ba37f5e..bd15eaaada 100644
--- a/modules/recast/register_types.h
+++ b/modules/gdnavigation/register_types.h
@@ -28,5 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
-void register_recast_types();
-void unregister_recast_types();
+/**
+ @author AndreaCatania
+*/
+
+void register_gdnavigation_types();
+void unregister_gdnavigation_types();
diff --git a/modules/gdnavigation/rvo_agent.cpp b/modules/gdnavigation/rvo_agent.cpp
new file mode 100644
index 0000000000..37a9b31351
--- /dev/null
+++ b/modules/gdnavigation/rvo_agent.cpp
@@ -0,0 +1,84 @@
+/*************************************************************************/
+/* rvo_agent.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* 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 "rvo_agent.h"
+
+#include "nav_map.h"
+
+/**
+ @author AndreaCatania
+*/
+
+RvoAgent::RvoAgent() :
+ map(NULL) {
+ callback.id = ObjectID(0);
+}
+
+void RvoAgent::set_map(NavMap *p_map) {
+ map = p_map;
+}
+
+bool RvoAgent::is_map_changed() {
+ if (map) {
+ bool is_changed = map->get_map_update_id() != map_update_id;
+ map_update_id = map->get_map_update_id();
+ return is_changed;
+ } else {
+ return false;
+ }
+}
+
+void RvoAgent::set_callback(ObjectID p_id, const StringName p_method, const Variant p_udata) {
+ callback.id = p_id;
+ callback.method = p_method;
+ callback.udata = p_udata;
+}
+
+bool RvoAgent::has_callback() const {
+ return callback.id != 0;
+}
+
+void RvoAgent::dispatch_callback() {
+ if (callback.id == 0) {
+ return;
+ }
+ Object *obj = ObjectDB::get_instance(callback.id);
+ if (obj == NULL) {
+ callback.id = ObjectID(0);
+ }
+
+ Variant::CallError responseCallError;
+
+ callback.new_velocity = Vector3(agent.newVelocity_.x(), agent.newVelocity_.y(), agent.newVelocity_.z());
+
+ const Variant *vp[2] = { &callback.new_velocity, &callback.udata };
+ int argc = (callback.udata.get_type() == Variant::NIL) ? 1 : 2;
+ obj->call(callback.method, vp, argc, responseCallError);
+}
diff --git a/modules/gdnavigation/rvo_agent.h b/modules/gdnavigation/rvo_agent.h
new file mode 100644
index 0000000000..6a84c1b95c
--- /dev/null
+++ b/modules/gdnavigation/rvo_agent.h
@@ -0,0 +1,77 @@
+/*************************************************************************/
+/* rvo_agent.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef RVO_AGENT_H
+#define RVO_AGENT_H
+
+#include "core/object.h"
+#include "nav_rid.h"
+#include
+
+/**
+ @author AndreaCatania
+*/
+
+class NavMap;
+
+class RvoAgent : public NavRid {
+ struct AvoidanceComputedCallback {
+ ObjectID id;
+ StringName method;
+ Variant udata;
+ Variant new_velocity;
+ };
+
+ NavMap *map;
+ RVO::Agent agent;
+ AvoidanceComputedCallback callback;
+ uint32_t map_update_id;
+
+public:
+ RvoAgent();
+
+ void set_map(NavMap *p_map);
+ NavMap *get_map() {
+ return map;
+ }
+
+ RVO::Agent *get_agent() {
+ return &agent;
+ }
+
+ bool is_map_changed();
+
+ void set_callback(ObjectID p_id, const StringName p_method, const Variant p_udata = Variant());
+ bool has_callback() const;
+
+ void dispatch_callback();
+};
+
+#endif // RVO_AGENT_H
diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index 3d40220869..1e73a715db 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -36,6 +36,7 @@
#include "scene/resources/mesh_library.h"
#include "scene/resources/surface_tool.h"
#include "scene/scene_string_names.h"
+#include "servers/navigation_server.h"
#include "servers/visual_server.h"
bool GridMap::_set(const StringName &p_name, const Variant &p_value) {
@@ -418,12 +419,10 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
}
//erase navigation
- if (navigation) {
- for (Map::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
- navigation->navmesh_remove(E->get().id);
- }
- g.navmesh_ids.clear();
+ for (Map::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
+ NavigationServer::get_singleton()->free(E->get().region);
}
+ g.navmesh_ids.clear();
//erase multimeshes
@@ -498,9 +497,11 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
nm.xform = xform * mesh_library->get_item_navmesh_transform(c.item);
if (navigation) {
- nm.id = navigation->navmesh_add(navmesh, xform, this);
- } else {
- nm.id = -1;
+ RID region = NavigationServer::get_singleton()->region_create();
+ NavigationServer::get_singleton()->region_set_navmesh(region, navmesh);
+ NavigationServer::get_singleton()->region_set_transform(region, navigation->get_global_transform() * nm.xform);
+ NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
+ nm.region = region;
}
g.navmesh_ids[E->get()] = nm;
}
@@ -591,10 +592,14 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
if (navigation && mesh_library.is_valid()) {
for (Map::Element *F = g.navmesh_ids.front(); F; F = F->next()) {
- if (cell_map.has(F->key()) && F->get().id < 0) {
+ if (cell_map.has(F->key()) && F->get().region.is_valid() == false) {
Ref nm = mesh_library->get_item_navmesh(cell_map[F->key()].item);
if (nm.is_valid()) {
- F->get().id = navigation->navmesh_add(nm, F->get().xform, this);
+ RID region = NavigationServer::get_singleton()->region_create();
+ NavigationServer::get_singleton()->region_set_navmesh(region, nm);
+ NavigationServer::get_singleton()->region_set_transform(region, navigation->get_global_transform() * F->get().xform);
+ NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
+ F->get().region = region;
}
}
}
@@ -620,9 +625,9 @@ void GridMap::_octant_exit_world(const OctantKey &p_key) {
if (navigation) {
for (Map::Element *F = g.navmesh_ids.front(); F; F = F->next()) {
- if (F->get().id >= 0) {
- navigation->navmesh_remove(F->get().id);
- F->get().id = -1;
+ if (F->get().region.is_valid()) {
+ NavigationServer::get_singleton()->free(F->get().region);
+ F->get().region = RID();
}
}
}
@@ -640,13 +645,11 @@ void GridMap::_octant_clean_up(const OctantKey &p_key) {
PhysicsServer::get_singleton()->free(g.static_body);
- //erase navigation
- if (navigation) {
- for (Map::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
- navigation->navmesh_remove(E->get().id);
- }
- g.navmesh_ids.clear();
+ // Erase navigation
+ for (Map::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
+ NavigationServer::get_singleton()->free(E->get().region);
}
+ g.navmesh_ids.clear();
//erase multimeshes
diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h
index 705f4929e1..cc1c8c2923 100644
--- a/modules/gridmap/grid_map.h
+++ b/modules/gridmap/grid_map.h
@@ -91,7 +91,7 @@ class GridMap : public Spatial {
struct Octant {
struct NavMesh {
- int id;
+ RID region;
Transform xform;
};
diff --git a/modules/recast/SCsub b/modules/recast/SCsub
deleted file mode 100644
index 94d9968164..0000000000
--- a/modules/recast/SCsub
+++ /dev/null
@@ -1,33 +0,0 @@
-#!/usr/bin/env python
-
-Import('env')
-Import('env_modules')
-
-env_recast = env_modules.Clone()
-
-# Thirdparty source files
-if env['builtin_recast']:
- thirdparty_dir = "#thirdparty/recastnavigation/Recast/"
- thirdparty_sources = [
- "Source/Recast.cpp",
- "Source/RecastAlloc.cpp",
- "Source/RecastArea.cpp",
- "Source/RecastAssert.cpp",
- "Source/RecastContour.cpp",
- "Source/RecastFilter.cpp",
- "Source/RecastLayers.cpp",
- "Source/RecastMesh.cpp",
- "Source/RecastMeshDetail.cpp",
- "Source/RecastRasterization.cpp",
- "Source/RecastRegion.cpp",
- ]
- thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
-
- env_recast.Prepend(CPPPATH=[thirdparty_dir + "/Include"])
-
- env_thirdparty = env_recast.Clone()
- env_thirdparty.disable_warnings()
- env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources)
-
-# Godot source files
-env_recast.add_source_files(env.modules_sources, "*.cpp")
diff --git a/scene/2d/navigation_2d.cpp b/scene/2d/navigation_2d.cpp
index 9209cc5074..de01d97ad9 100644
--- a/scene/2d/navigation_2d.cpp
+++ b/scene/2d/navigation_2d.cpp
@@ -29,710 +29,52 @@
/*************************************************************************/
#include "navigation_2d.h"
+#include "servers/navigation_2d_server.h"
-#define USE_ENTRY_POINT
+void Navigation2D::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("get_rid"), &Navigation2D::get_rid);
-void Navigation2D::_navpoly_link(int p_id) {
+ ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- NavMesh &nm = navpoly_map[p_id];
- ERR_FAIL_COND(nm.linked);
+ ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation2D::set_cell_size);
+ ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation2D::get_cell_size);
- PoolVector vertices = nm.navpoly->get_vertices();
- int len = vertices.size();
- if (len == 0)
- return;
+ ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation2D::set_edge_connection_margin);
+ ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation2D::get_edge_connection_margin);
- PoolVector::Read r = vertices.read();
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
+}
- for (int i = 0; i < nm.navpoly->get_polygon_count(); i++) {
+void Navigation2D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+ Navigation2DServer::get_singleton()->map_set_active(map, true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
- //build
-
- List::Element *P = nm.polygons.push_back(Polygon());
- Polygon &p = P->get();
- p.owner = &nm;
-
- Vector poly = nm.navpoly->get_polygon(i);
- int plen = poly.size();
- const int *indices = poly.ptr();
- bool valid = true;
- p.edges.resize(plen);
-
- Vector2 center;
- float sum = 0;
-
- for (int j = 0; j < plen; j++) {
-
- int idx = indices[j];
- if (idx < 0 || idx >= len) {
- valid = false;
- break;
- }
-
- Polygon::Edge e;
- Vector2 ep = nm.xform.xform(r[idx]);
- center += ep;
- e.point = _get_point(ep);
- p.edges.write[j] = e;
-
- int idxn = indices[(j + 1) % plen];
- if (idxn < 0 || idxn >= len) {
- valid = false;
- break;
- }
-
- Vector2 epn = nm.xform.xform(r[idxn]);
-
- sum += (epn.x - ep.x) * (epn.y + ep.y);
- }
-
- p.clockwise = sum > 0;
-
- if (!valid) {
- nm.polygons.pop_back();
- ERR_CONTINUE(!valid);
- }
-
- p.center = center / plen;
-
- //connect
-
- for (int j = 0; j < plen; j++) {
-
- int next = (j + 1) % plen;
- EdgeKey ek(p.edges[j].point, p.edges[next].point);
-
- Map::Element *C = connections.find(ek);
- if (!C) {
-
- Connection c;
- c.A = &p;
- c.A_edge = j;
- c.B = NULL;
- c.B_edge = -1;
- connections[ek] = c;
- } else {
-
- if (C->get().B != NULL) {
- ConnectionPending pending;
- pending.polygon = &p;
- pending.edge = j;
- p.edges.write[j].P = C->get().pending.push_back(pending);
- continue;
- }
-
- C->get().B = &p;
- C->get().B_edge = j;
- C->get().A->edges.write[C->get().A_edge].C = &p;
- C->get().A->edges.write[C->get().A_edge].C_edge = j;
- p.edges.write[j].C = C->get().A;
- p.edges.write[j].C_edge = C->get().A_edge;
- //connection successful.
- }
- }
+ Navigation2DServer::get_singleton()->map_set_active(map, false);
+ } break;
}
-
- nm.linked = true;
}
-void Navigation2D::_navpoly_unlink(int p_id) {
-
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- NavMesh &nm = navpoly_map[p_id];
- ERR_FAIL_COND(!nm.linked);
-
- for (List::Element *E = nm.polygons.front(); E; E = E->next()) {
-
- Polygon &p = E->get();
-
- int ec = p.edges.size();
- Polygon::Edge *edges = p.edges.ptrw();
-
- for (int i = 0; i < ec; i++) {
- int next = (i + 1) % ec;
-
- EdgeKey ek(edges[i].point, edges[next].point);
- Map::Element *C = connections.find(ek);
- ERR_CONTINUE(!C);
-
- if (edges[i].P) {
- C->get().pending.erase(edges[i].P);
- edges[i].P = NULL;
-
- } else if (C->get().B) {
- //disconnect
-
- C->get().B->edges.write[C->get().B_edge].C = NULL;
- C->get().B->edges.write[C->get().B_edge].C_edge = -1;
- C->get().A->edges.write[C->get().A_edge].C = NULL;
- C->get().A->edges.write[C->get().A_edge].C_edge = -1;
-
- if (C->get().A == &E->get()) {
-
- C->get().A = C->get().B;
- C->get().A_edge = C->get().B_edge;
- }
- C->get().B = NULL;
- C->get().B_edge = -1;
-
- if (C->get().pending.size()) {
- //reconnect if something is pending
- ConnectionPending cp = C->get().pending.front()->get();
- C->get().pending.pop_front();
-
- C->get().B = cp.polygon;
- C->get().B_edge = cp.edge;
- C->get().A->edges.write[C->get().A_edge].C = cp.polygon;
- C->get().A->edges.write[C->get().A_edge].C_edge = cp.edge;
- cp.polygon->edges.write[cp.edge].C = C->get().A;
- cp.polygon->edges.write[cp.edge].C_edge = C->get().A_edge;
- cp.polygon->edges.write[cp.edge].P = NULL;
- }
-
- } else {
- connections.erase(C);
- //erase
- }
- }
- }
-
- nm.polygons.clear();
-
- nm.linked = false;
+void Navigation2D::set_cell_size(float p_cell_size) {
+ cell_size = p_cell_size;
+ Navigation2DServer::get_singleton()->map_set_cell_size(map, cell_size);
}
-int Navigation2D::navpoly_add(const Ref &p_mesh, const Transform2D &p_xform, Object *p_owner) {
-
- int id = last_id++;
- NavMesh nm;
- nm.linked = false;
- nm.navpoly = p_mesh;
- nm.xform = p_xform;
- nm.owner = p_owner;
- navpoly_map[id] = nm;
-
- _navpoly_link(id);
-
- return id;
-}
-
-void Navigation2D::navpoly_set_transform(int p_id, const Transform2D &p_xform) {
-
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- NavMesh &nm = navpoly_map[p_id];
- if (nm.xform == p_xform)
- return; //bleh
- _navpoly_unlink(p_id);
- nm.xform = p_xform;
- _navpoly_link(p_id);
-}
-void Navigation2D::navpoly_remove(int p_id) {
-
- ERR_FAIL_COND(!navpoly_map.has(p_id));
- _navpoly_unlink(p_id);
- navpoly_map.erase(p_id);
+void Navigation2D::set_edge_connection_margin(float p_edge_connection_margin) {
+ edge_connection_margin = p_edge_connection_margin;
+ Navigation2DServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
}
Vector Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) {
-
- Polygon *begin_poly = NULL;
- Polygon *end_poly = NULL;
- Vector2 begin_point;
- Vector2 end_point;
- float begin_d = 1e20;
- float end_d = 1e20;
-
- //look for point inside triangle
-
- for (Map::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- if (begin_d || end_d) {
- for (int i = 2; i < p.edges.size(); i++) {
-
- if (begin_d > 0) {
-
- if (Geometry::is_point_in_triangle(p_start, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- begin_poly = &p;
- begin_point = p_start;
- begin_d = 0;
- if (end_d == 0)
- break;
- }
- }
-
- if (end_d > 0) {
-
- if (Geometry::is_point_in_triangle(p_end, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- end_poly = &p;
- end_point = p_end;
- end_d = 0;
- if (begin_d == 0)
- break;
- }
- }
- }
- }
-
- p.prev_edge = -1;
- }
- }
-
- //start or end not inside triangle.. look for closest segment :|
- if (begin_d || end_d) {
- for (Map::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- int es = p.edges.size();
- for (int i = 0; i < es; i++) {
-
- Vector2 edge[2] = {
- _get_vertex(p.edges[i].point),
- _get_vertex(p.edges[(i + 1) % es].point)
- };
-
- if (begin_d > 0) {
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_start, edge);
- float d = spoint.distance_to(p_start);
- if (d < begin_d) {
- begin_poly = &p;
- begin_point = spoint;
- begin_d = d;
- }
- }
-
- if (end_d > 0) {
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_end, edge);
- float d = spoint.distance_to(p_end);
- if (d < end_d) {
- end_poly = &p;
- end_point = spoint;
- end_d = d;
- }
- }
- }
- }
- }
- }
-
- if (!begin_poly || !end_poly) {
-
- return Vector(); //no path
- }
-
- if (begin_poly == end_poly) {
-
- Vector path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
- }
-
- bool found_route = false;
-
- List open_list;
-
- begin_poly->entry = p_start;
-
- for (int i = 0; i < begin_poly->edges.size(); i++) {
-
- if (begin_poly->edges[i].C) {
-
- begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
-#ifdef USE_ENTRY_POINT
- Vector2 edge[2] = {
- _get_vertex(begin_poly->edges[i].point),
- _get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
- };
-
- Vector2 entry = Geometry::get_closest_point_to_segment_2d(begin_poly->entry, edge);
- begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
- begin_poly->edges[i].C->entry = entry;
-#else
- begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
-#endif
- open_list.push_back(begin_poly->edges[i].C);
-
- if (begin_poly->edges[i].C == end_poly) {
- found_route = true;
- }
- }
- }
-
- while (!found_route) {
-
- if (open_list.size() == 0) {
- break;
- }
- //check open list
-
- List::Element *least_cost_poly = NULL;
- float least_cost = 1e30;
-
- //this could be faster (cache previous results)
- for (List::Element *E = open_list.front(); E; E = E->next()) {
-
- Polygon *p = E->get();
-
- float cost = p->distance;
-
-#ifdef USE_ENTRY_POINT
- int es = p->edges.size();
-
- float shortest_distance = 1e30;
-
- for (int i = 0; i < es; i++) {
- Polygon::Edge &e = p->edges.write[i];
-
- if (!e.C)
- continue;
-
- Vector2 edge[2] = {
- _get_vertex(p->edges[i].point),
- _get_vertex(p->edges[(i + 1) % es].point)
- };
-
- Vector2 edge_point = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
- float dist = p->entry.distance_to(edge_point);
- if (dist < shortest_distance)
- shortest_distance = dist;
- }
-
- cost += shortest_distance;
-#else
- cost += p->center.distance_to(end_point);
-#endif
- if (cost < least_cost) {
- least_cost_poly = E;
- least_cost = cost;
- }
- }
-
- Polygon *p = least_cost_poly->get();
- //open the neighbours for search
- int es = p->edges.size();
-
- for (int i = 0; i < es; i++) {
-
- Polygon::Edge &e = p->edges.write[i];
-
- if (!e.C)
- continue;
-
-#ifdef USE_ENTRY_POINT
- Vector2 edge[2] = {
- _get_vertex(p->edges[i].point),
- _get_vertex(p->edges[(i + 1) % es].point)
- };
-
- Vector2 edge_entry = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
- float distance = p->entry.distance_to(edge_entry) + p->distance;
-
-#else
-
- float distance = p->center.distance_to(e.C->center) + p->distance;
-
-#endif
-
- if (e.C->prev_edge != -1) {
- //oh this was visited already, can we win the cost?
-
- if (e.C->distance > distance) {
-
- e.C->prev_edge = e.C_edge;
- e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
- e.C->entry = edge_entry;
-#endif
- }
- } else {
- //add to open neighbours
-
- e.C->prev_edge = e.C_edge;
- e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
- e.C->entry = edge_entry;
-#endif
-
- open_list.push_back(e.C);
-
- if (e.C == end_poly) {
- //oh my reached end! stop algorithm
- found_route = true;
- break;
- }
- }
- }
-
- if (found_route)
- break;
-
- open_list.erase(least_cost_poly);
- }
-
- if (found_route) {
-
- Vector path;
-
- if (p_optimize) {
- //string pulling
-
- Vector2 apex_point = end_point;
- Vector2 portal_left = apex_point;
- Vector2 portal_right = apex_point;
- Polygon *left_poly = end_poly;
- Polygon *right_poly = end_poly;
- Polygon *p = end_poly;
-
- while (p) {
-
- Vector2 left;
- Vector2 right;
-
-//#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
-#define CLOCK_TANGENT(m_a, m_b, m_c) ((((m_a).x - (m_c).x) * ((m_b).y - (m_c).y) - ((m_b).x - (m_c).x) * ((m_a).y - (m_c).y)))
-
- if (p == begin_poly) {
- left = begin_point;
- right = begin_point;
- } else {
- int prev = p->prev_edge;
- int prev_n = (p->prev_edge + 1) % p->edges.size();
- left = _get_vertex(p->edges[prev].point);
- right = _get_vertex(p->edges[prev_n].point);
-
- if (p->clockwise) {
- SWAP(left, right);
- }
- /*if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
- SWAP(left,right);
- }*/
- }
-
- bool skip = false;
-
- /*
- print_line("-----\nAPEX: "+(apex_point-end_point));
- print_line("LEFT:");
- print_line("\tPortal: "+(portal_left-end_point));
- print_line("\tPoint: "+(left-end_point));
- print_line("\tLeft Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_left,left)));
- print_line("\tLeft Distance: "+rtos(portal_left.distance_squared_to(apex_point)));
- print_line("\tLeft Test: "+rtos(CLOCK_TANGENT(apex_point,left,portal_right)));
- print_line("RIGHT:");
- print_line("\tPortal: "+(portal_right-end_point));
- print_line("\tPoint: "+(right-end_point));
- print_line("\tRight Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_right,right)));
- print_line("\tRight Distance: "+rtos(portal_right.distance_squared_to(apex_point)));
- print_line("\tRight Test: "+rtos(CLOCK_TANGENT(apex_point,right,portal_left)));
- */
-
- if (CLOCK_TANGENT(apex_point, portal_left, left) >= 0) {
- //process
- if (portal_left.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
- left_poly = p;
- portal_left = left;
- } else {
-
- apex_point = portal_right;
- p = right_poly;
- left_poly = p;
- portal_left = apex_point;
- portal_right = apex_point;
- if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
- path.push_back(apex_point);
- skip = true;
- }
- }
-
- if (!skip && CLOCK_TANGENT(apex_point, portal_right, right) <= 0) {
- //process
- if (portal_right.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
- right_poly = p;
- portal_right = right;
- } else {
-
- apex_point = portal_left;
- p = left_poly;
- right_poly = p;
- portal_right = apex_point;
- portal_left = apex_point;
- if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
- path.push_back(apex_point);
- }
- }
-
- if (p != begin_poly)
- p = p->edges[p->prev_edge].C;
- else
- p = NULL;
- }
-
- } else {
- //midpoints
- Polygon *p = end_poly;
-
- while (true) {
- int prev = p->prev_edge;
- int prev_n = (p->prev_edge + 1) % p->edges.size();
- Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
- path.push_back(point);
- p = p->edges[prev].C;
- if (p == begin_poly)
- break;
- }
- }
-
- if (!path.size() || !path[path.size() - 1].is_equal_approx(begin_point)) {
- path.push_back(begin_point); // Add the begin point
- } else {
- path.write[path.size() - 1] = begin_point; // Replace first midpoint by the exact begin point
- }
-
- path.invert();
-
- if (path.size() <= 1 || !path[path.size() - 1].is_equal_approx(end_point)) {
- path.push_back(end_point); // Add the end point
- } else {
- path.write[path.size() - 1] = end_point; // Replace last midpoint by the exact end point
- }
-
- return path;
- }
-
- return Vector();
-}
-
-Vector2 Navigation2D::get_closest_point(const Vector2 &p_point) {
-
- Vector2 closest_point = Vector2();
- float closest_point_d = 1e20;
-
- for (Map::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- return p_point; //inside triangle, nothing else to discuss
- }
- }
- }
- }
-
- for (Map::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- int es = p.edges.size();
- for (int i = 0; i < es; i++) {
-
- Vector2 edge[2] = {
- _get_vertex(p.edges[i].point),
- _get_vertex(p.edges[(i + 1) % es].point)
- };
-
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
- float d = spoint.distance_squared_to(p_point);
- if (d < closest_point_d) {
-
- closest_point = spoint;
- closest_point_d = d;
- }
- }
- }
- }
-
- return closest_point;
-}
-
-Object *Navigation2D::get_closest_point_owner(const Vector2 &p_point) {
-
- Object *owner = NULL;
- Vector2 closest_point = Vector2();
- float closest_point_d = 1e20;
-
- for (Map::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
- return E->get().owner;
- }
- }
- }
- }
-
- for (Map::Element *E = navpoly_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- int es = p.edges.size();
- for (int i = 0; i < es; i++) {
-
- Vector2 edge[2] = {
- _get_vertex(p.edges[i].point),
- _get_vertex(p.edges[(i + 1) % es].point)
- };
-
- Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
- float d = spoint.distance_squared_to(p_point);
- if (d < closest_point_d) {
-
- closest_point = spoint;
- closest_point_d = d;
- owner = E->get().owner;
- }
- }
- }
- }
-
- return owner;
-}
-
-void Navigation2D::_bind_methods() {
-
- ClassDB::bind_method(D_METHOD("navpoly_add", "mesh", "xform", "owner"), &Navigation2D::navpoly_add, DEFVAL(Variant()));
- ClassDB::bind_method(D_METHOD("navpoly_set_transform", "id", "xform"), &Navigation2D::navpoly_set_transform);
- ClassDB::bind_method(D_METHOD("navpoly_remove", "id"), &Navigation2D::navpoly_remove);
-
- ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
- ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation2D::get_closest_point);
- ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner);
+ return Navigation2DServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
}
Navigation2D::Navigation2D() {
- ERR_FAIL_COND(sizeof(Point) != 8);
- cell_size = 1; // one pixel
- last_id = 1;
+ map = Navigation2DServer::get_singleton()->map_create();
+ set_cell_size(10); // Ten pixels
+ set_edge_connection_margin(100);
}
diff --git a/scene/2d/navigation_2d.h b/scene/2d/navigation_2d.h
index 41b9aea35a..08642a5489 100644
--- a/scene/2d/navigation_2d.h
+++ b/scene/2d/navigation_2d.h
@@ -38,135 +38,30 @@ class Navigation2D : public Node2D {
GDCLASS(Navigation2D, Node2D);
- union Point {
-
- struct {
- int64_t x : 32;
- int64_t y : 32;
- };
-
- uint64_t key;
- bool operator<(const Point &p_key) const { return key < p_key.key; }
- };
-
- struct EdgeKey {
-
- Point a;
- Point b;
-
- bool operator<(const EdgeKey &p_key) const {
- return (a.key == p_key.a.key) ? (b.key < p_key.b.key) : (a.key < p_key.a.key);
- };
-
- EdgeKey(const Point &p_a = Point(), const Point &p_b = Point()) :
- a(p_a),
- b(p_b) {
- if (a.key > b.key) {
- SWAP(a, b);
- }
- }
- };
-
- struct NavMesh;
- struct Polygon;
-
- struct ConnectionPending {
-
- Polygon *polygon;
- int edge;
- };
-
- struct Polygon {
-
- struct Edge {
- Point point;
- Polygon *C; //connection
- int C_edge;
- List::Element *P;
- Edge() {
- C = NULL;
- C_edge = -1;
- P = NULL;
- }
- };
-
- Vector edges;
-
- Vector2 center;
- Vector2 entry;
-
- float distance;
- int prev_edge;
-
- bool clockwise;
-
- NavMesh *owner;
- };
-
- struct Connection {
-
- Polygon *A;
- int A_edge;
- Polygon *B;
- int B_edge;
-
- List pending;
-
- Connection() {
- A = NULL;
- B = NULL;
- A_edge = -1;
- B_edge = -1;
- }
- };
-
- Map connections;
-
- struct NavMesh {
-
- Object *owner;
- Transform2D xform;
- bool linked;
- Ref navpoly;
- List polygons;
- };
-
- _FORCE_INLINE_ Point _get_point(const Vector2 &p_pos) const {
-
- int x = int(Math::floor(p_pos.x / cell_size));
- int y = int(Math::floor(p_pos.y / cell_size));
-
- Point p;
- p.key = 0;
- p.x = x;
- p.y = y;
- return p;
- }
-
- _FORCE_INLINE_ Vector2 _get_vertex(const Point &p_point) const {
-
- return Vector2(p_point.x, p_point.y) * cell_size;
- }
-
- void _navpoly_link(int p_id);
- void _navpoly_unlink(int p_id);
-
- float cell_size;
- Map navpoly_map;
- int last_id;
+ RID map;
+ real_t cell_size;
+ real_t edge_connection_margin;
protected:
static void _bind_methods();
+ void _notification(int p_what);
public:
- //API should be as dynamic as possible
- int navpoly_add(const Ref &p_mesh, const Transform2D &p_xform, Object *p_owner = NULL);
- void navpoly_set_transform(int p_id, const Transform2D &p_xform);
- void navpoly_remove(int p_id);
+ RID get_rid() const {
+ return map;
+ }
+
+ void set_cell_size(float p_cell_size);
+ float get_cell_size() const {
+ return cell_size;
+ }
+
+ void set_edge_connection_margin(float p_edge_connection_margin);
+ float get_edge_connection_margin() const {
+ return edge_connection_margin;
+ }
Vector get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize = true);
- Vector2 get_closest_point(const Vector2 &p_point);
- Object *get_closest_point_owner(const Vector2 &p_point);
Navigation2D();
};
diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
new file mode 100644
index 0000000000..fe4d3b322f
--- /dev/null
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -0,0 +1,341 @@
+/*************************************************************************/
+/* navigation_agent_2d.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "navigation_agent_2d.h"
+
+#include "core/engine.h"
+#include "scene/2d/navigation_2d.h"
+#include "servers/navigation_2d_server.h"
+
+void NavigationAgent2D::_bind_methods() {
+
+ ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent2D::set_target_desired_distance);
+ ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent2D::get_target_desired_distance);
+
+ ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent2D::set_radius);
+ ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent2D::get_radius);
+
+ ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent2D::set_navigation_node);
+ ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent2D::get_navigation_node);
+
+ ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent2D::set_neighbor_dist);
+ ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent2D::get_neighbor_dist);
+
+ ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent2D::set_max_neighbors);
+ ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent2D::get_max_neighbors);
+
+ ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent2D::set_time_horizon);
+ ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent2D::get_time_horizon);
+
+ ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent2D::set_max_speed);
+ ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent2D::get_max_speed);
+
+ ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent2D::set_path_max_distance);
+ ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent2D::get_path_max_distance);
+
+ ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent2D::set_target_location);
+ ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent2D::get_target_location);
+ ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent2D::get_next_location);
+ ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target);
+ ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent2D::set_velocity);
+ ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent2D::get_nav_path);
+ ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent2D::get_nav_path_index);
+ ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent2D::is_target_reached);
+ ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent2D::is_target_reachable);
+ ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent2D::is_navigation_finished);
+ ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent2D::get_final_location);
+
+ ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent2D::_avoidance_done);
+
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,500,0.01"), "set_radius", "get_radius");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_neighbor_dist", "get_neighbor_dist");
+ ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1"), "set_max_neighbors", "get_max_neighbors");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "time_horizon", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_time_horizon", "get_time_horizon");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_max_speed", "get_max_speed");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "10,100,1"), "set_path_max_distance", "get_path_max_distance");
+
+ ADD_SIGNAL(MethodInfo("path_changed"));
+ ADD_SIGNAL(MethodInfo("target_reached"));
+ ADD_SIGNAL(MethodInfo("navigation_finished"));
+ ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity")));
+}
+
+void NavigationAgent2D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+
+ agent_parent = Object::cast_to(get_parent());
+
+ Navigation2DServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
+
+ // Search the navigation node and set it
+ {
+ Navigation2D *nav = NULL;
+ Node *p = get_parent();
+ while (p != NULL) {
+ nav = Object::cast_to(p);
+ if (nav != NULL)
+ p = NULL;
+ else
+ p = p->get_parent();
+ }
+
+ set_navigation(nav);
+ }
+
+ set_physics_process_internal(true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+ agent_parent = NULL;
+ set_navigation(NULL);
+ set_physics_process_internal(false);
+ } break;
+ case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+ if (agent_parent) {
+
+ Navigation2DServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().get_origin());
+ if (!target_reached) {
+ if (distance_to_target() < target_desired_distance) {
+ emit_signal("target_reached");
+ target_reached = true;
+ }
+ }
+ }
+ } break;
+ }
+}
+
+NavigationAgent2D::NavigationAgent2D() :
+ agent_parent(NULL),
+ navigation(NULL),
+ agent(RID()),
+ target_desired_distance(1.0),
+ path_max_distance(3.0),
+ velocity_submitted(false),
+ target_reached(false),
+ navigation_finished(true) {
+ agent = Navigation2DServer::get_singleton()->agent_create();
+ set_neighbor_dist(500.0);
+ set_max_neighbors(10);
+ set_time_horizon(20.0);
+ set_radius(10.0);
+ set_max_speed(200.0);
+}
+
+NavigationAgent2D::~NavigationAgent2D() {
+ Navigation2DServer::get_singleton()->free(agent);
+ agent = RID(); // Pointless
+}
+
+void NavigationAgent2D::set_navigation(Navigation2D *p_nav) {
+ if (navigation == p_nav)
+ return; // Pointless
+
+ navigation = p_nav;
+ Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationAgent2D::set_navigation_node(Node *p_nav) {
+ Navigation2D *nav = Object::cast_to(p_nav);
+ ERR_FAIL_COND(nav == NULL);
+ set_navigation(nav);
+}
+
+Node *NavigationAgent2D::get_navigation_node() const {
+ return Object::cast_to(navigation);
+}
+
+void NavigationAgent2D::set_target_desired_distance(real_t p_dd) {
+ target_desired_distance = p_dd;
+}
+
+void NavigationAgent2D::set_radius(real_t p_radius) {
+ radius = p_radius;
+ Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
+}
+
+void NavigationAgent2D::set_neighbor_dist(real_t p_dist) {
+ neighbor_dist = p_dist;
+ Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
+}
+
+void NavigationAgent2D::set_max_neighbors(int p_count) {
+ max_neighbors = p_count;
+ Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
+}
+
+void NavigationAgent2D::set_time_horizon(real_t p_time) {
+ time_horizon = p_time;
+ Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
+}
+
+void NavigationAgent2D::set_max_speed(real_t p_max_speed) {
+ max_speed = p_max_speed;
+ Navigation2DServer::get_singleton()->agent_set_max_speed(agent, max_speed);
+}
+
+void NavigationAgent2D::set_path_max_distance(real_t p_pmd) {
+ path_max_distance = p_pmd;
+}
+
+real_t NavigationAgent2D::get_path_max_distance() {
+ return path_max_distance;
+}
+
+void NavigationAgent2D::set_target_location(Vector2 p_location) {
+ target_location = p_location;
+ navigation_path.clear();
+ target_reached = false;
+ navigation_finished = false;
+}
+
+Vector2 NavigationAgent2D::get_target_location() const {
+ return target_location;
+}
+
+Vector2 NavigationAgent2D::get_next_location() {
+ update_navigation();
+ if (navigation_path.size() == 0) {
+ ERR_FAIL_COND_V(agent_parent == NULL, Vector2());
+ return agent_parent->get_global_transform().get_origin();
+ } else {
+ return navigation_path[nav_path_index];
+ }
+}
+
+real_t NavigationAgent2D::distance_to_target() const {
+ ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
+ return agent_parent->get_global_transform().get_origin().distance_to(target_location);
+}
+
+bool NavigationAgent2D::is_target_reached() const {
+ return target_reached;
+}
+
+bool NavigationAgent2D::is_target_reachable() {
+ return target_desired_distance >= get_final_location().distance_to(target_location);
+}
+
+bool NavigationAgent2D::is_navigation_finished() {
+ update_navigation();
+ return navigation_finished;
+}
+
+Vector2 NavigationAgent2D::get_final_location() {
+ update_navigation();
+ if (navigation_path.size() == 0) {
+ return Vector2();
+ }
+ return navigation_path[navigation_path.size() - 1];
+}
+
+void NavigationAgent2D::set_velocity(Vector2 p_velocity) {
+ target_velocity = p_velocity;
+ Navigation2DServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
+ Navigation2DServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
+ velocity_submitted = true;
+}
+
+void NavigationAgent2D::_avoidance_done(Vector3 p_new_velocity) {
+ const Vector2 velocity = Vector2(p_new_velocity.x, p_new_velocity.z);
+ prev_safe_velocity = velocity;
+
+ if (!velocity_submitted) {
+ target_velocity = Vector2();
+ return;
+ }
+ velocity_submitted = false;
+
+ emit_signal("velocity_computed", velocity);
+}
+
+String NavigationAgent2D::get_configuration_warning() const {
+ if (!Object::cast_to(get_parent())) {
+ return TTR("The NavigationAgent2D can be used only under a Node2D node");
+ }
+
+ return String();
+}
+
+void NavigationAgent2D::update_navigation() {
+
+ if (agent_parent == NULL) return;
+ if (navigation == NULL) return;
+ if (update_frame_id == Engine::get_singleton()->get_physics_frames()) return;
+
+ update_frame_id = Engine::get_singleton()->get_physics_frames();
+
+ Vector2 o = agent_parent->get_global_transform().get_origin();
+
+ bool reload_path = false;
+
+ if (Navigation2DServer::get_singleton()->agent_is_map_changed(agent)) {
+ reload_path = true;
+ } else if (navigation_path.size() == 0) {
+ reload_path = true;
+ } else {
+ // Check if too far from the navigation path
+ if (nav_path_index > 0) {
+ Vector2 segment[2];
+ segment[0] = navigation_path[nav_path_index - 1];
+ segment[1] = navigation_path[nav_path_index];
+ Vector2 p = Geometry::get_closest_point_to_segment_2d(o, segment);
+ if (o.distance_to(p) >= path_max_distance) {
+ // To faraway, reload path
+ reload_path = true;
+ }
+ }
+ }
+
+ if (reload_path) {
+ navigation_path = Navigation2DServer::get_singleton()->map_get_path(navigation->get_rid(), o, target_location, true);
+ navigation_finished = false;
+ nav_path_index = 0;
+ emit_signal("path_changed");
+ }
+
+ if (navigation_path.size() == 0)
+ return;
+
+ // Check if we can advance the navigation path
+ if (navigation_finished == false) {
+ // Advances to the next far away location.
+ while (o.distance_to(navigation_path[nav_path_index]) < target_desired_distance) {
+ nav_path_index += 1;
+ if (nav_path_index == navigation_path.size()) {
+ nav_path_index -= 1;
+ navigation_finished = true;
+ emit_signal("navigation_finished");
+ break;
+ }
+ }
+ }
+}
diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h
new file mode 100644
index 0000000000..60527091ce
--- /dev/null
+++ b/scene/2d/navigation_agent_2d.h
@@ -0,0 +1,150 @@
+/*************************************************************************/
+/* navigation_agent_2d.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAVIGATION_AGENT_2D_H
+#define NAVIGATION_AGENT_2D_H
+
+#include "core/vector.h"
+#include "scene/main/node.h"
+
+class Node2D;
+class Navigation2D;
+
+class NavigationAgent2D : public Node {
+ GDCLASS(NavigationAgent2D, Node);
+
+ Node2D *agent_parent;
+ Navigation2D *navigation;
+
+ RID agent;
+
+ real_t target_desired_distance;
+ real_t radius;
+ real_t neighbor_dist;
+ int max_neighbors;
+ real_t time_horizon;
+ real_t max_speed;
+
+ real_t path_max_distance;
+
+ Vector2 target_location;
+ Vector navigation_path;
+ int nav_path_index;
+ bool velocity_submitted;
+ Vector2 prev_safe_velocity;
+ /// The submitted target velocity
+ Vector2 target_velocity;
+ bool target_reached;
+ bool navigation_finished;
+ // No initialized on purpose
+ uint32_t update_frame_id;
+
+protected:
+ static void _bind_methods();
+ void _notification(int p_what);
+
+public:
+ NavigationAgent2D();
+ virtual ~NavigationAgent2D();
+
+ void set_navigation(Navigation2D *p_nav);
+ const Navigation2D *get_navigation() const {
+ return navigation;
+ }
+
+ void set_navigation_node(Node *p_nav);
+ Node *get_navigation_node() const;
+
+ RID get_rid() const {
+ return agent;
+ }
+
+ void set_target_desired_distance(real_t p_dd);
+ real_t get_target_desired_distance() const {
+ return target_desired_distance;
+ }
+
+ void set_radius(real_t p_radius);
+ real_t get_radius() const {
+ return radius;
+ }
+
+ void set_neighbor_dist(real_t p_dist);
+ real_t get_neighbor_dist() const {
+ return neighbor_dist;
+ }
+
+ void set_max_neighbors(int p_count);
+ int get_max_neighbors() const {
+ return max_neighbors;
+ }
+
+ void set_time_horizon(real_t p_time);
+ real_t get_time_horizon() const {
+ return time_horizon;
+ }
+
+ void set_max_speed(real_t p_max_speed);
+ real_t get_max_speed() const {
+ return max_speed;
+ }
+
+ void set_path_max_distance(real_t p_pmd);
+ real_t get_path_max_distance();
+
+ void set_target_location(Vector2 p_location);
+ Vector2 get_target_location() const;
+
+ Vector2 get_next_location();
+
+ Vector get_nav_path() const {
+ return navigation_path;
+ }
+
+ int get_nav_path_index() const {
+ return nav_path_index;
+ }
+
+ real_t distance_to_target() const;
+ bool is_target_reached() const;
+ bool is_target_reachable();
+ bool is_navigation_finished();
+ Vector2 get_final_location();
+
+ void set_velocity(Vector2 p_velocity);
+ void _avoidance_done(Vector3 p_new_velocity);
+
+ virtual String get_configuration_warning() const;
+
+private:
+ void update_navigation();
+};
+
+#endif
diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
new file mode 100644
index 0000000000..0740b616b6
--- /dev/null
+++ b/scene/2d/navigation_obstacle_2d.cpp
@@ -0,0 +1,154 @@
+/*************************************************************************/
+/* navigation_obstacle.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "navigation_obstacle_2d.h"
+
+#include "scene/2d/collision_shape_2d.h"
+#include "scene/2d/navigation_2d.h"
+#include "scene/2d/physics_body_2d.h"
+#include "servers/navigation_2d_server.h"
+
+void NavigationObstacle2D::_bind_methods() {
+
+ ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle2D::set_navigation_node);
+ ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle2D::get_navigation_node);
+}
+
+void NavigationObstacle2D::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+
+ update_agent_shape();
+
+ // Search the navigation node and set it
+ {
+ Navigation2D *nav = NULL;
+ Node *p = get_parent();
+ while (p != NULL) {
+ nav = Object::cast_to(p);
+ if (nav != NULL)
+ p = NULL;
+ else
+ p = p->get_parent();
+ }
+
+ set_navigation(nav);
+ }
+
+ set_physics_process_internal(true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+ set_navigation(NULL);
+ set_physics_process_internal(false);
+ } break;
+ case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+ Node2D *node = Object::cast_to(get_parent());
+ if (node) {
+ Navigation2DServer::get_singleton()->agent_set_position(agent, node->get_global_transform().get_origin());
+ }
+
+ } break;
+ }
+}
+
+NavigationObstacle2D::NavigationObstacle2D() :
+ navigation(NULL),
+ agent(RID()) {
+ agent = Navigation2DServer::get_singleton()->agent_create();
+}
+
+NavigationObstacle2D::~NavigationObstacle2D() {
+ Navigation2DServer::get_singleton()->free(agent);
+ agent = RID(); // Pointless
+}
+
+void NavigationObstacle2D::set_navigation(Navigation2D *p_nav) {
+ if (navigation == p_nav)
+ return; // Pointless
+
+ navigation = p_nav;
+ Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationObstacle2D::set_navigation_node(Node *p_nav) {
+ Navigation2D *nav = Object::cast_to(p_nav);
+ ERR_FAIL_COND(nav == NULL);
+ set_navigation(nav);
+}
+
+Node *NavigationObstacle2D::get_navigation_node() const {
+ return Object::cast_to(navigation);
+}
+
+String NavigationObstacle2D::get_configuration_warning() const {
+ if (!Object::cast_to(get_parent())) {
+ return TTR("The NavigationObstacle2D only serves to provide collision avoidance to a Node2D object.");
+ }
+
+ return String();
+}
+
+void NavigationObstacle2D::update_agent_shape() {
+ Node *node = get_parent();
+
+ // Estimate the radius of this physics body
+ real_t radius = 0.0;
+ for (int i(0); i < node->get_child_count(); i++) {
+ // For each collision shape
+ CollisionShape2D *cs = Object::cast_to(node->get_child(i));
+ if (cs) {
+ // Take the distance between the Body center to the shape center
+ real_t r = cs->get_transform().get_origin().length();
+ if (cs->get_shape().is_valid()) {
+ // and add the enclosing shape radius
+ r += cs->get_shape()->get_enclosing_radius();
+ }
+ Size2 s = cs->get_global_transform().get_scale();
+ r *= MAX(s.x, s.y);
+ // Takes the biggest radius
+ radius = MAX(radius, r);
+ }
+ }
+ Node2D *node_2d = Object::cast_to(node);
+ if (node_2d) {
+ Vector2 s = node_2d->get_global_transform().get_scale();
+ radius *= MAX(s.x, s.y);
+ }
+
+ if (radius == 0.0)
+ radius = 1.0; // Never a 0 radius
+
+ // Initialize the Agent as an object
+ Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, 0.0);
+ Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, 0);
+ Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, 0.0);
+ Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
+ Navigation2DServer::get_singleton()->agent_set_max_speed(agent, 0.0);
+}
diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h
new file mode 100644
index 0000000000..f098255799
--- /dev/null
+++ b/scene/2d/navigation_obstacle_2d.h
@@ -0,0 +1,71 @@
+/*************************************************************************/
+/* navigation_obstacle.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAVIGATION_OBSTACLE_2D_H
+#define NAVIGATION_OBSTACLE_2D_H
+
+#include "scene/main/node.h"
+
+class Navigation2D;
+
+class NavigationObstacle2D : public Node {
+ GDCLASS(NavigationObstacle2D, Node);
+
+ Navigation2D *navigation;
+
+ RID agent;
+
+protected:
+ static void _bind_methods();
+ void _notification(int p_what);
+
+public:
+ NavigationObstacle2D();
+ virtual ~NavigationObstacle2D();
+
+ void set_navigation(Navigation2D *p_nav);
+ const Navigation2D *get_navigation() const {
+ return navigation;
+ }
+
+ void set_navigation_node(Node *p_nav);
+ Node *get_navigation_node() const;
+
+ RID get_rid() const {
+ return agent;
+ }
+
+ virtual String get_configuration_warning() const;
+
+private:
+ void update_agent_shape();
+};
+
+#endif
diff --git a/scene/2d/navigation_polygon.cpp b/scene/2d/navigation_polygon.cpp
index 883fb37668..c40cdb4720 100644
--- a/scene/2d/navigation_polygon.cpp
+++ b/scene/2d/navigation_polygon.cpp
@@ -32,7 +32,9 @@
#include "core/core_string_names.h"
#include "core/engine.h"
+#include "core/os/mutex.h"
#include "navigation_2d.h"
+#include "servers/navigation_2d_server.h"
#include "thirdparty/misc/triangulator.h"
@@ -80,6 +82,9 @@ bool NavigationPolygon::_edit_is_selected_on_click(const Point2 &p_point, double
void NavigationPolygon::set_vertices(const PoolVector &p_vertices) {
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
vertices = p_vertices;
rect_cache_dirty = true;
}
@@ -91,6 +96,9 @@ PoolVector NavigationPolygon::get_vertices() const {
void NavigationPolygon::_set_polygons(const Array &p_array) {
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
polygons.resize(p_array.size());
for (int i = 0; i < p_array.size(); i++) {
polygons.write[i].indices = p_array[i];
@@ -133,6 +141,9 @@ void NavigationPolygon::add_polygon(const Vector &p_polygon) {
Polygon polygon;
polygon.indices = p_polygon;
polygons.push_back(polygon);
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
}
void NavigationPolygon::add_outline_at_index(const PoolVector &p_outline, int p_index) {
@@ -153,6 +164,34 @@ Vector NavigationPolygon::get_polygon(int p_idx) {
void NavigationPolygon::clear_polygons() {
polygons.clear();
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
+}
+
+Ref NavigationPolygon::get_mesh() {
+ navmesh_generation->lock();
+ if (navmesh.is_null()) {
+ navmesh.instance();
+ PoolVector verts;
+ {
+ verts.resize(get_vertices().size());
+ PoolVector::Write w = verts.write();
+
+ PoolVector::Read r = get_vertices().read();
+
+ for (int i(0); i < get_vertices().size(); i++) {
+ w[i] = Vector3(r[i].x, 0.0, r[i].y);
+ }
+ }
+ navmesh->set_vertices(verts);
+
+ for (int i(0); i < get_polygon_count(); i++) {
+ navmesh->add_polygon(get_polygon(i));
+ }
+ }
+ navmesh_generation->unlock();
+ return navmesh;
}
void NavigationPolygon::add_outline(const PoolVector &p_outline) {
@@ -191,6 +230,9 @@ void NavigationPolygon::clear_outlines() {
}
void NavigationPolygon::make_polygons_from_outlines() {
+ navmesh_generation->lock();
+ navmesh.unref();
+ navmesh_generation->unlock();
List in_poly, out_poly;
Vector2 outside_point(-1e10, -1e10);
@@ -320,7 +362,9 @@ void NavigationPolygon::_bind_methods() {
}
NavigationPolygon::NavigationPolygon() :
- rect_cache_dirty(true) {
+ rect_cache_dirty(true),
+ navmesh_generation(NULL) {
+ navmesh_generation = Mutex::create();
}
void NavigationPolygonInstance::set_enabled(bool p_enabled) {
@@ -334,18 +378,12 @@ void NavigationPolygonInstance::set_enabled(bool p_enabled) {
if (!enabled) {
- if (nav_id != -1) {
- navigation->navpoly_remove(nav_id);
- nav_id = -1;
- }
+ Navigation2DServer::get_singleton()->region_set_map(region, RID());
} else {
if (navigation) {
- if (navpoly.is_valid()) {
-
- nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
- }
+ Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
}
@@ -382,9 +420,9 @@ void NavigationPolygonInstance::_notification(int p_what) {
navigation = Object::cast_to(c);
if (navigation) {
- if (enabled && navpoly.is_valid()) {
+ if (enabled) {
- nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
+ Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
break;
}
@@ -395,19 +433,14 @@ void NavigationPolygonInstance::_notification(int p_what) {
} break;
case NOTIFICATION_TRANSFORM_CHANGED: {
- if (navigation && nav_id != -1) {
- navigation->navpoly_set_transform(nav_id, get_relative_transform_to_parent(navigation));
- }
+ Navigation2DServer::get_singleton()->region_set_transform(region, get_global_transform());
} break;
case NOTIFICATION_EXIT_TREE: {
if (navigation) {
- if (nav_id != -1) {
- navigation->navpoly_remove(nav_id);
- nav_id = -1;
- }
+ Navigation2DServer::get_singleton()->region_set_map(region, RID());
}
navigation = NULL;
} break;
@@ -466,24 +499,18 @@ void NavigationPolygonInstance::set_navigation_polygon(const Refnavpoly_remove(nav_id);
- nav_id = -1;
- }
-
if (navpoly.is_valid()) {
navpoly->disconnect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
}
+
navpoly = p_navpoly;
+ Navigation2DServer::get_singleton()->region_set_navpoly(region, p_navpoly);
+
if (navpoly.is_valid()) {
navpoly->connect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
}
_navpoly_changed();
- if (navigation && navpoly.is_valid() && enabled) {
- nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
- }
-
_change_notify("navpoly");
update_configuration_warning();
}
@@ -536,8 +563,13 @@ void NavigationPolygonInstance::_bind_methods() {
NavigationPolygonInstance::NavigationPolygonInstance() {
- navigation = NULL;
- nav_id = -1;
enabled = true;
set_notify_transform(true);
+ region = Navigation2DServer::get_singleton()->region_create();
+
+ navigation = NULL;
+}
+
+NavigationPolygonInstance::~NavigationPolygonInstance() {
+ Navigation2DServer::get_singleton()->free(region);
}
diff --git a/scene/2d/navigation_polygon.h b/scene/2d/navigation_polygon.h
index cbc1711a32..92cfc8f5e6 100644
--- a/scene/2d/navigation_polygon.h
+++ b/scene/2d/navigation_polygon.h
@@ -32,6 +32,9 @@
#define NAVIGATION_POLYGON_H
#include "scene/2d/node_2d.h"
+#include "scene/resources/navigation_mesh.h"
+
+class Mutex;
class NavigationPolygon : public Resource {
@@ -47,6 +50,10 @@ class NavigationPolygon : public Resource {
mutable Rect2 item_rect;
mutable bool rect_cache_dirty;
+ Mutex *navmesh_generation;
+ // Navigation mesh
+ Ref navmesh;
+
protected:
static void _bind_methods();
@@ -81,6 +88,8 @@ public:
Vector get_polygon(int p_idx);
void clear_polygons();
+ Ref get_mesh();
+
NavigationPolygon();
};
@@ -91,7 +100,7 @@ class NavigationPolygonInstance : public Node2D {
GDCLASS(NavigationPolygonInstance, Node2D);
bool enabled;
- int nav_id;
+ RID region;
Navigation2D *navigation;
Ref navpoly;
@@ -116,6 +125,7 @@ public:
String get_configuration_warning() const;
NavigationPolygonInstance();
+ ~NavigationPolygonInstance();
};
#endif // NAVIGATIONPOLYGON_H
diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp
index b6db025d44..d8b880e571 100644
--- a/scene/2d/tile_map.cpp
+++ b/scene/2d/tile_map.cpp
@@ -35,6 +35,7 @@
#include "core/method_bind_ext.gen.inc"
#include "core/os/os.h"
#include "scene/2d/area_2d.h"
+#include "servers/navigation_2d_server.h"
#include "servers/physics_2d_server.h"
int TileMap::_get_quadrant_size() const {
@@ -86,7 +87,7 @@ void TileMap::_notification(int p_what) {
if (navigation) {
for (Map::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
- navigation->navpoly_remove(F->get().id);
+ Navigation2DServer::get_singleton()->region_set_map(F->get().region, RID());
}
q.navpoly_ids.clear();
}
@@ -163,7 +164,7 @@ void TileMap::_update_quadrant_transform() {
if (navigation) {
for (Map::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
- navigation->navpoly_set_transform(F->get().id, nav_rel * F->get().xform);
+ Navigation2DServer::get_singleton()->region_set_transform(F->get().region, nav_rel * F->get().xform);
}
}
@@ -377,7 +378,7 @@ void TileMap::update_dirty_quadrants() {
if (navigation) {
for (Map::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
- navigation->navpoly_remove(E->get().id);
+ Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
}
q.navpoly_ids.clear();
}
@@ -611,10 +612,13 @@ void TileMap::update_dirty_quadrants() {
xform.set_origin(offset.floor() + q.pos);
_fix_cell_transform(xform, c, npoly_ofs, s);
- int pid = navigation->navpoly_add(navpoly, nav_rel * xform);
+ RID region = Navigation2DServer::get_singleton()->region_create();
+ Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
+ Navigation2DServer::get_singleton()->region_set_transform(region, nav_rel * xform);
+ Navigation2DServer::get_singleton()->region_set_navpoly(region, navpoly);
Quadrant::NavPoly np;
- np.id = pid;
+ np.region = region;
np.xform = xform;
q.navpoly_ids[E->key()] = np;
@@ -809,7 +813,7 @@ void TileMap::_erase_quadrant(Map::Element *Q) {
if (navigation) {
for (Map::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
- navigation->navpoly_remove(E->get().id);
+ Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
}
q.navpoly_ids.clear();
}
diff --git a/scene/2d/tile_map.h b/scene/2d/tile_map.h
index 0875d197eb..d5ef7fc818 100644
--- a/scene/2d/tile_map.h
+++ b/scene/2d/tile_map.h
@@ -139,7 +139,7 @@ private:
SelfList dirty_list;
struct NavPoly {
- int id;
+ RID region;
Transform2D xform;
};
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 4a82fe0080..10b12f5c75 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -30,669 +30,17 @@
#include "navigation.h"
-#define USE_ENTRY_POINT
-
-void Navigation::_navmesh_link(int p_id) {
-
- ERR_FAIL_COND(!navmesh_map.has(p_id));
- NavMesh &nm = navmesh_map[p_id];
- ERR_FAIL_COND(nm.linked);
- ERR_FAIL_COND(nm.navmesh.is_null());
-
- PoolVector vertices = nm.navmesh->get_vertices();
- int len = vertices.size();
- if (len == 0)
- return;
-
- PoolVector::Read r = vertices.read();
-
- for (int i = 0; i < nm.navmesh->get_polygon_count(); i++) {
-
- //build
-
- List::Element *P = nm.polygons.push_back(Polygon());
- Polygon &p = P->get();
- p.owner = &nm;
-
- Vector poly = nm.navmesh->get_polygon(i);
- int plen = poly.size();
- const int *indices = poly.ptr();
- bool valid = true;
- p.edges.resize(plen);
-
- Vector3 center;
- float sum = 0;
-
- for (int j = 0; j < plen; j++) {
-
- int idx = indices[j];
- if (idx < 0 || idx >= len) {
- valid = false;
- break;
- }
-
- Polygon::Edge e;
- Vector3 ep = nm.xform.xform(r[idx]);
- center += ep;
- e.point = _get_point(ep);
- p.edges.write[j] = e;
-
- if (j >= 2) {
- Vector3 epa = nm.xform.xform(r[indices[j - 2]]);
- Vector3 epb = nm.xform.xform(r[indices[j - 1]]);
-
- sum += up.dot((epb - epa).cross(ep - epa));
- }
- }
-
- p.clockwise = sum > 0;
-
- if (!valid) {
- nm.polygons.pop_back();
- ERR_CONTINUE(!valid);
- }
-
- p.center = center;
- if (plen != 0) {
- p.center /= plen;
- }
-
- //connect
-
- for (int j = 0; j < plen; j++) {
-
- int next = (j + 1) % plen;
- EdgeKey ek(p.edges[j].point, p.edges[next].point);
-
- Map::Element *C = connections.find(ek);
- if (!C) {
-
- Connection c;
- c.A = &p;
- c.A_edge = j;
- c.B = NULL;
- c.B_edge = -1;
- connections[ek] = c;
- } else {
-
- if (C->get().B != NULL) {
- ConnectionPending pending;
- pending.polygon = &p;
- pending.edge = j;
- p.edges.write[j].P = C->get().pending.push_back(pending);
- continue;
- }
-
- C->get().B = &p;
- C->get().B_edge = j;
- C->get().A->edges.write[C->get().A_edge].C = &p;
- C->get().A->edges.write[C->get().A_edge].C_edge = j;
- p.edges.write[j].C = C->get().A;
- p.edges.write[j].C_edge = C->get().A_edge;
- //connection successful.
- }
- }
- }
-
- nm.linked = true;
-}
-
-void Navigation::_navmesh_unlink(int p_id) {
-
- ERR_FAIL_COND(!navmesh_map.has(p_id));
- NavMesh &nm = navmesh_map[p_id];
- ERR_FAIL_COND(!nm.linked);
-
- for (List::Element *E = nm.polygons.front(); E; E = E->next()) {
-
- Polygon &p = E->get();
-
- int ec = p.edges.size();
- Polygon::Edge *edges = p.edges.ptrw();
-
- for (int i = 0; i < ec; i++) {
- int next = (i + 1) % ec;
-
- EdgeKey ek(edges[i].point, edges[next].point);
- Map::Element *C = connections.find(ek);
-
- ERR_CONTINUE(!C);
-
- if (edges[i].P) {
- C->get().pending.erase(edges[i].P);
- edges[i].P = NULL;
- } else if (C->get().B) {
- //disconnect
-
- C->get().B->edges.write[C->get().B_edge].C = NULL;
- C->get().B->edges.write[C->get().B_edge].C_edge = -1;
- C->get().A->edges.write[C->get().A_edge].C = NULL;
- C->get().A->edges.write[C->get().A_edge].C_edge = -1;
-
- if (C->get().A == &E->get()) {
-
- C->get().A = C->get().B;
- C->get().A_edge = C->get().B_edge;
- }
- C->get().B = NULL;
- C->get().B_edge = -1;
-
- if (C->get().pending.size()) {
- //reconnect if something is pending
- ConnectionPending cp = C->get().pending.front()->get();
- C->get().pending.pop_front();
-
- C->get().B = cp.polygon;
- C->get().B_edge = cp.edge;
- C->get().A->edges.write[C->get().A_edge].C = cp.polygon;
- C->get().A->edges.write[C->get().A_edge].C_edge = cp.edge;
- cp.polygon->edges.write[cp.edge].C = C->get().A;
- cp.polygon->edges.write[cp.edge].C_edge = C->get().A_edge;
- cp.polygon->edges.write[cp.edge].P = NULL;
- }
-
- } else {
- connections.erase(C);
- //erase
- }
- }
- }
-
- nm.polygons.clear();
-
- nm.linked = false;
-}
-
-int Navigation::navmesh_add(const Ref &p_mesh, const Transform &p_xform, Object *p_owner) {
-
- int id = last_id++;
- NavMesh nm;
- nm.linked = false;
- nm.navmesh = p_mesh;
- nm.xform = p_xform;
- nm.owner = p_owner;
- navmesh_map[id] = nm;
-
- _navmesh_link(id);
-
- return id;
-}
-
-void Navigation::navmesh_set_transform(int p_id, const Transform &p_xform) {
-
- ERR_FAIL_COND(!navmesh_map.has(p_id));
- NavMesh &nm = navmesh_map[p_id];
- if (nm.xform == p_xform)
- return; //bleh
- _navmesh_unlink(p_id);
- nm.xform = p_xform;
- _navmesh_link(p_id);
-}
-void Navigation::navmesh_remove(int p_id) {
-
- ERR_FAIL_COND_MSG(!navmesh_map.has(p_id), "Trying to remove nonexisting navmesh with id: " + itos(p_id));
- _navmesh_unlink(p_id);
- navmesh_map.erase(p_id);
-}
-
-void Navigation::_clip_path(Vector &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly) {
-
- Vector3 from = path[path.size() - 1];
-
- if (from.distance_to(p_to_point) < CMP_EPSILON)
- return;
- Plane cut_plane;
- cut_plane.normal = (from - p_to_point).cross(up);
- if (cut_plane.normal == Vector3())
- return;
- cut_plane.normal.normalize();
- cut_plane.d = cut_plane.normal.dot(from);
-
- while (from_poly != p_to_poly) {
-
- int pe = from_poly->prev_edge;
- Vector3 a = _get_vertex(from_poly->edges[pe].point);
- Vector3 b = _get_vertex(from_poly->edges[(pe + 1) % from_poly->edges.size()].point);
-
- from_poly = from_poly->edges[pe].C;
- ERR_FAIL_COND(!from_poly);
-
- if (a.distance_to(b) > CMP_EPSILON) {
-
- Vector3 inters;
- if (cut_plane.intersects_segment(a, b, &inters)) {
- if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
- path.push_back(inters);
- }
- }
- }
- }
-}
+#include "servers/navigation_server.h"
Vector Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) {
- Polygon *begin_poly = NULL;
- Polygon *end_poly = NULL;
- Vector3 begin_point;
- Vector3 end_point;
- float begin_d = 1e20;
- float end_d = 1e20;
-
- for (Map::Element *E = navmesh_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
- Vector3 spoint = f.get_closest_point_to(p_start);
- float dpoint = spoint.distance_to(p_start);
- if (dpoint < begin_d) {
- begin_d = dpoint;
- begin_poly = &p;
- begin_point = spoint;
- }
-
- spoint = f.get_closest_point_to(p_end);
- dpoint = spoint.distance_to(p_end);
- if (dpoint < end_d) {
- end_d = dpoint;
- end_poly = &p;
- end_point = spoint;
- }
- }
-
- p.prev_edge = -1;
- }
- }
-
- if (!begin_poly || !end_poly) {
-
- return Vector(); //no path
- }
-
- if (begin_poly == end_poly) {
-
- Vector path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
- }
-
- bool found_route = false;
-
- List open_list;
-
- for (int i = 0; i < begin_poly->edges.size(); i++) {
-
- if (begin_poly->edges[i].C) {
-
- begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
-#ifdef USE_ENTRY_POINT
- Vector3 edge[2] = {
- _get_vertex(begin_poly->edges[i].point),
- _get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
- };
-
- Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
- begin_poly->edges[i].C->distance = begin_point.distance_to(entry);
- begin_poly->edges[i].C->entry = entry;
-#else
- begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
-#endif
- open_list.push_back(begin_poly->edges[i].C);
- }
- }
-
- while (!found_route) {
-
- if (open_list.size() == 0) {
- break;
- }
- //check open list
-
- List::Element *least_cost_poly = NULL;
- float least_cost = 1e30;
-
- //this could be faster (cache previous results)
- for (List::Element *E = open_list.front(); E; E = E->next()) {
-
- Polygon *p = E->get();
-
- float cost = p->distance;
-#ifdef USE_ENTRY_POINT
- cost += p->entry.distance_to(end_point);
-#else
- cost += p->center.distance_to(end_point);
-#endif
- if (cost < least_cost) {
- least_cost_poly = E;
- least_cost = cost;
- }
- }
-
- Polygon *p = least_cost_poly->get();
- //open the neighbours for search
-
- if (p == end_poly) {
- //oh my reached end! stop algorithm
- found_route = true;
- break;
- }
-
- for (int i = 0; i < p->edges.size(); i++) {
-
- Polygon::Edge &e = p->edges.write[i];
-
- if (!e.C)
- continue;
-
-#ifdef USE_ENTRY_POINT
- Vector3 edge[2] = {
- _get_vertex(p->edges[i].point),
- _get_vertex(p->edges[(i + 1) % p->edges.size()].point)
- };
-
- Vector3 entry = Geometry::get_closest_point_to_segment(p->entry, edge);
- float distance = p->entry.distance_to(entry) + p->distance;
-#else
- float distance = p->center.distance_to(e.C->center) + p->distance;
-#endif
-
- if (e.C->prev_edge != -1) {
- //oh this was visited already, can we win the cost?
-
- if (e.C->distance > distance) {
-
- e.C->prev_edge = e.C_edge;
- e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
- e.C->entry = entry;
-#endif
- }
- } else {
- //add to open neighbours
-
- e.C->prev_edge = e.C_edge;
- e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
- e.C->entry = entry;
-#endif
- open_list.push_back(e.C);
- }
- }
-
- open_list.erase(least_cost_poly);
- }
-
- if (found_route) {
-
- Vector path;
-
- if (p_optimize) {
- //string pulling
-
- Polygon *apex_poly = end_poly;
- Vector3 apex_point = end_point;
- Vector3 portal_left = apex_point;
- Vector3 portal_right = apex_point;
- Polygon *left_poly = end_poly;
- Polygon *right_poly = end_poly;
- Polygon *p = end_poly;
- path.push_back(end_point);
-
- while (p) {
-
- Vector3 left;
- Vector3 right;
-
-#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b)))
-
- if (p == begin_poly) {
- left = begin_point;
- right = begin_point;
- } else {
- int prev = p->prev_edge;
- int prev_n = (p->prev_edge + 1) % p->edges.size();
- left = _get_vertex(p->edges[prev].point);
- right = _get_vertex(p->edges[prev_n].point);
-
- //if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){
- if (p->clockwise) {
- SWAP(left, right);
- }
- }
-
- bool skip = false;
-
- if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) {
- //process
- if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) {
- left_poly = p;
- portal_left = left;
- } else {
-
- _clip_path(path, apex_poly, portal_right, right_poly);
-
- apex_point = portal_right;
- p = right_poly;
- left_poly = p;
- apex_poly = p;
- portal_left = apex_point;
- portal_right = apex_point;
- path.push_back(apex_point);
- skip = true;
- }
- }
-
- if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) {
- //process
- if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) {
- right_poly = p;
- portal_right = right;
- } else {
-
- _clip_path(path, apex_poly, portal_left, left_poly);
-
- apex_point = portal_left;
- p = left_poly;
- right_poly = p;
- apex_poly = p;
- portal_right = apex_point;
- portal_left = apex_point;
- path.push_back(apex_point);
- }
- }
-
- if (p != begin_poly)
- p = p->edges[p->prev_edge].C;
- else
- p = NULL;
- }
-
- if (path[path.size() - 1] != begin_point)
- path.push_back(begin_point);
-
- path.invert();
-
- } else {
- //midpoints
- Polygon *p = end_poly;
-
- path.push_back(end_point);
- while (true) {
- int prev = p->prev_edge;
-#ifdef USE_ENTRY_POINT
- Vector3 point = p->entry;
-#else
- int prev_n = (p->prev_edge + 1) % p->edges.size();
- Vector3 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
-#endif
- path.push_back(point);
- p = p->edges[prev].C;
- if (p == begin_poly)
- break;
- }
-
- path.push_back(begin_point);
-
- path.invert();
- }
-
- return path;
- }
-
- return Vector();
-}
-
-Vector3 Navigation::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision) {
-
- bool use_collision = p_use_collision;
- Vector3 closest_point;
- float closest_point_d = 1e20;
-
- for (Map::Element *E = navmesh_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
- Vector3 inters;
- if (f.intersects_segment(p_from, p_to, &inters)) {
-
- if (!use_collision) {
- closest_point = inters;
- use_collision = true;
- closest_point_d = p_from.distance_to(inters);
- } else if (closest_point_d > inters.distance_to(p_from)) {
-
- closest_point = inters;
- closest_point_d = p_from.distance_to(inters);
- }
- }
- }
-
- if (!use_collision) {
-
- for (int i = 0; i < p.edges.size(); i++) {
-
- Vector3 a, b;
-
- Geometry::get_closest_points_between_segments(p_from, p_to, _get_vertex(p.edges[i].point), _get_vertex(p.edges[(i + 1) % p.edges.size()].point), a, b);
-
- float d = a.distance_to(b);
- if (d < closest_point_d) {
-
- closest_point_d = d;
- closest_point = b;
- }
- }
- }
- }
- }
-
- return closest_point;
-}
-
-Vector3 Navigation::get_closest_point(const Vector3 &p_point) {
-
- Vector3 closest_point;
- float closest_point_d = 1e20;
-
- for (Map::Element *E = navmesh_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
- Vector3 inters = f.get_closest_point_to(p_point);
- float d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_d = d;
- }
- }
- }
- }
-
- return closest_point;
-}
-
-Vector3 Navigation::get_closest_point_normal(const Vector3 &p_point) {
-
- Vector3 closest_point;
- Vector3 closest_normal;
- float closest_point_d = 1e20;
-
- for (Map::Element *E = navmesh_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
- Vector3 inters = f.get_closest_point_to(p_point);
- float d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_d = d;
- closest_normal = f.get_plane().normal;
- }
- }
- }
- }
-
- return closest_normal;
-}
-
-Object *Navigation::get_closest_point_owner(const Vector3 &p_point) {
-
- Vector3 closest_point;
- Object *owner = NULL;
- float closest_point_d = 1e20;
-
- for (Map::Element *E = navmesh_map.front(); E; E = E->next()) {
-
- if (!E->get().linked)
- continue;
- for (List::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
- Polygon &p = F->get();
- for (int i = 2; i < p.edges.size(); i++) {
-
- Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
- Vector3 inters = f.get_closest_point_to(p_point);
- float d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_d = d;
- owner = E->get().owner;
- }
- }
- }
- }
-
- return owner;
+ return NavigationServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
}
void Navigation::set_up_vector(const Vector3 &p_up) {
up = p_up;
+ NavigationServer::get_singleton()->map_set_up(map, up);
}
Vector3 Navigation::get_up_vector() const {
@@ -700,28 +48,58 @@ Vector3 Navigation::get_up_vector() const {
return up;
}
+void Navigation::set_cell_size(float p_cell_size) {
+ cell_size = p_cell_size;
+ NavigationServer::get_singleton()->map_set_cell_size(map, cell_size);
+}
+
+void Navigation::set_edge_connection_margin(float p_edge_connection_margin) {
+ edge_connection_margin = p_edge_connection_margin;
+ NavigationServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
+}
+
void Navigation::_bind_methods() {
- ClassDB::bind_method(D_METHOD("navmesh_add", "mesh", "xform", "owner"), &Navigation::navmesh_add, DEFVAL(Variant()));
- ClassDB::bind_method(D_METHOD("navmesh_set_transform", "id", "xform"), &Navigation::navmesh_set_transform);
- ClassDB::bind_method(D_METHOD("navmesh_remove", "id"), &Navigation::navmesh_remove);
+ ClassDB::bind_method(D_METHOD("get_rid"), &Navigation::get_rid);
ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation::get_simple_path, DEFVAL(true));
- ClassDB::bind_method(D_METHOD("get_closest_point_to_segment", "start", "end", "use_collision"), &Navigation::get_closest_point_to_segment, DEFVAL(false));
- ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation::get_closest_point);
- ClassDB::bind_method(D_METHOD("get_closest_point_normal", "to_point"), &Navigation::get_closest_point_normal);
- ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation::get_closest_point_owner);
ClassDB::bind_method(D_METHOD("set_up_vector", "up"), &Navigation::set_up_vector);
ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector);
+ ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation::set_cell_size);
+ ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation::get_cell_size);
+
+ ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation::set_edge_connection_margin);
+ ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation::get_edge_connection_margin);
+
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "get_up_vector");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
+}
+
+void Navigation::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+ NavigationServer::get_singleton()->map_set_active(map, true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+
+ NavigationServer::get_singleton()->map_set_active(map, false);
+ } break;
+ }
}
Navigation::Navigation() {
- ERR_FAIL_COND(sizeof(Point) != 8);
- cell_size = 0.01; //one centimeter
- last_id = 1;
+ map = NavigationServer::get_singleton()->map_create();
+
+ set_cell_size(0.3);
+ set_edge_connection_margin(5.0); // Five meters, depends alot on the agents radius
+
up = Vector3(0, 1, 0);
}
+
+Navigation::~Navigation() {
+ NavigationServer::get_singleton()->free(map);
+}
diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h
index 31dbc9d4b5..68e041ad73 100644
--- a/scene/3d/navigation.h
+++ b/scene/3d/navigation.h
@@ -31,154 +31,45 @@
#ifndef NAVIGATION_H
#define NAVIGATION_H
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/spatial.h"
class Navigation : public Spatial {
GDCLASS(Navigation, Spatial);
- union Point {
-
- struct {
- int64_t x : 21;
- int64_t y : 22;
- int64_t z : 21;
- };
-
- uint64_t key;
- bool operator<(const Point &p_key) const { return key < p_key.key; }
- };
-
- struct EdgeKey {
-
- Point a;
- Point b;
-
- bool operator<(const EdgeKey &p_key) const {
- return (a.key == p_key.a.key) ? (b.key < p_key.b.key) : (a.key < p_key.a.key);
- };
-
- EdgeKey(const Point &p_a = Point(), const Point &p_b = Point()) :
- a(p_a),
- b(p_b) {
- if (a.key > b.key) {
- SWAP(a, b);
- }
- }
- };
-
- struct NavMesh;
- struct Polygon;
-
- struct ConnectionPending {
-
- Polygon *polygon;
- int edge;
- };
-
- struct Polygon {
-
- struct Edge {
- Point point;
- Polygon *C; //connection
- int C_edge;
- List::Element *P;
- Edge() {
- C = NULL;
- C_edge = -1;
- P = NULL;
- }
- };
-
- Vector edges;
-
- Vector3 center;
- Vector3 entry;
-
- float distance;
- int prev_edge;
- bool clockwise;
-
- NavMesh *owner;
- };
-
- struct Connection {
-
- Polygon *A;
- int A_edge;
- Polygon *B;
- int B_edge;
-
- List pending;
-
- Connection() {
- A = NULL;
- B = NULL;
- A_edge = -1;
- B_edge = -1;
- }
- };
-
- Map connections;
-
- struct NavMesh {
-
- Object *owner;
- Transform xform;
- bool linked;
- Ref navmesh;
- List polygons;
- };
-
- _FORCE_INLINE_ Point _get_point(const Vector3 &p_pos) const {
-
- int x = int(Math::floor(p_pos.x / cell_size));
- int y = int(Math::floor(p_pos.y / cell_size));
- int z = int(Math::floor(p_pos.z / cell_size));
-
- Point p;
- p.key = 0;
- p.x = x;
- p.y = y;
- p.z = z;
- return p;
- }
-
- _FORCE_INLINE_ Vector3 _get_vertex(const Point &p_point) const {
-
- return Vector3(p_point.x, p_point.y, p_point.z) * cell_size;
- }
-
- void _navmesh_link(int p_id);
- void _navmesh_unlink(int p_id);
-
- float cell_size;
- Map navmesh_map;
- int last_id;
+ RID map;
Vector3 up;
- void _clip_path(Vector &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly);
+ real_t cell_size;
+ real_t edge_connection_margin;
protected:
static void _bind_methods();
+ void _notification(int p_what);
public:
+ RID get_rid() const {
+ return map;
+ }
+
void set_up_vector(const Vector3 &p_up);
Vector3 get_up_vector() const;
- //API should be as dynamic as possible
- int navmesh_add(const Ref &p_mesh, const Transform &p_xform, Object *p_owner = NULL);
- void navmesh_set_transform(int p_id, const Transform &p_xform);
- void navmesh_remove(int p_id);
+ void set_cell_size(float p_cell_size);
+ float get_cell_size() const {
+ return cell_size;
+ }
+
+ void set_edge_connection_margin(float p_edge_connection_margin);
+ float get_edge_connection_margin() const {
+ return edge_connection_margin;
+ }
Vector get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize = true);
- Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision = false);
- Vector3 get_closest_point(const Vector3 &p_point);
- Vector3 get_closest_point_normal(const Vector3 &p_point);
- Object *get_closest_point_owner(const Vector3 &p_point);
Navigation();
+ ~Navigation();
};
#endif // NAVIGATION_H
diff --git a/scene/3d/navigation_agent.cpp b/scene/3d/navigation_agent.cpp
new file mode 100644
index 0000000000..d3f447bc24
--- /dev/null
+++ b/scene/3d/navigation_agent.cpp
@@ -0,0 +1,361 @@
+/*************************************************************************/
+/* navigation_agent.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "navigation_agent.h"
+
+#include "core/engine.h"
+#include "scene/3d/navigation.h"
+#include "servers/navigation_server.h"
+
+void NavigationAgent::_bind_methods() {
+
+ ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent::set_target_desired_distance);
+ ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent::get_target_desired_distance);
+
+ ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent::set_radius);
+ ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent::get_radius);
+
+ ClassDB::bind_method(D_METHOD("set_agent_height_offset", "agent_height_offset"), &NavigationAgent::set_agent_height_offset);
+ ClassDB::bind_method(D_METHOD("get_agent_height_offset"), &NavigationAgent::get_agent_height_offset);
+
+ ClassDB::bind_method(D_METHOD("set_ignore_y", "ignore"), &NavigationAgent::set_ignore_y);
+ ClassDB::bind_method(D_METHOD("get_ignore_y"), &NavigationAgent::get_ignore_y);
+
+ ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent::set_navigation_node);
+ ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent::get_navigation_node);
+
+ ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent::set_neighbor_dist);
+ ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent::get_neighbor_dist);
+
+ ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent::set_max_neighbors);
+ ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent::get_max_neighbors);
+
+ ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent::set_time_horizon);
+ ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent::get_time_horizon);
+
+ ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent::set_max_speed);
+ ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent::get_max_speed);
+
+ ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent::set_path_max_distance);
+ ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent::get_path_max_distance);
+
+ ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent::set_target_location);
+ ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent::get_target_location);
+ ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent::get_next_location);
+ ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent::distance_to_target);
+ ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent::set_velocity);
+ ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent::get_nav_path);
+ ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent::get_nav_path_index);
+ ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent::is_target_reached);
+ ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent::is_target_reachable);
+ ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent::is_navigation_finished);
+ ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent::get_final_location);
+
+ ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent::_avoidance_done);
+
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_radius", "get_radius");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent_height_offset", PROPERTY_HINT_RANGE, "-100.0,100,0.01"), "set_agent_height_offset", "get_agent_height_offset");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_neighbor_dist", "get_neighbor_dist");
+ ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1"), "set_max_neighbors", "get_max_neighbors");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "time_horizon", PROPERTY_HINT_RANGE, "0.01,100,0.01"), "set_time_horizon", "get_time_horizon");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_max_speed", "get_max_speed");
+ ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1"), "set_path_max_distance", "get_path_max_distance");
+ ADD_PROPERTY(PropertyInfo(Variant::BOOL, "ignore_y"), "set_ignore_y", "get_ignore_y");
+
+ ADD_SIGNAL(MethodInfo("path_changed"));
+ ADD_SIGNAL(MethodInfo("target_reached"));
+ ADD_SIGNAL(MethodInfo("navigation_finished"));
+ ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity")));
+}
+
+void NavigationAgent::_notification(int p_what) {
+ switch (p_what) {
+ case NOTIFICATION_READY: {
+
+ agent_parent = Object::cast_to(get_parent());
+
+ NavigationServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
+
+ // Search the navigation node and set it
+ {
+ Navigation *nav = NULL;
+ Node *p = get_parent();
+ while (p != NULL) {
+ nav = Object::cast_to(p);
+ if (nav != NULL)
+ p = NULL;
+ else
+ p = p->get_parent();
+ }
+
+ set_navigation(nav);
+ }
+
+ set_physics_process_internal(true);
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+ agent_parent = NULL;
+ set_navigation(NULL);
+ set_physics_process_internal(false);
+ } break;
+ case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+ if (agent_parent) {
+
+ NavigationServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
+ if (!target_reached) {
+ if (distance_to_target() < target_desired_distance) {
+ emit_signal("target_reached");
+ target_reached = true;
+ }
+ }
+ }
+ } break;
+ }
+}
+
+NavigationAgent::NavigationAgent() :
+ agent_parent(NULL),
+ navigation(NULL),
+ agent(RID()),
+ target_desired_distance(1.0),
+ navigation_height_offset(0.0),
+ path_max_distance(3.0),
+ velocity_submitted(false),
+ target_reached(false),
+ navigation_finished(true) {
+ agent = NavigationServer::get_singleton()->agent_create();
+ set_neighbor_dist(50.0);
+ set_max_neighbors(10);
+ set_time_horizon(5.0);
+ set_radius(1.0);
+ set_max_speed(10.0);
+ set_ignore_y(true);
+}
+
+NavigationAgent::~NavigationAgent() {
+ NavigationServer::get_singleton()->free(agent);
+ agent = RID(); // Pointless
+}
+
+void NavigationAgent::set_navigation(Navigation *p_nav) {
+ if (navigation == p_nav)
+ return; // Pointless
+
+ navigation = p_nav;
+ NavigationServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationAgent::set_navigation_node(Node *p_nav) {
+ Navigation *nav = Object::cast_to(p_nav);
+ ERR_FAIL_COND(nav == NULL);
+ set_navigation(nav);
+}
+
+Node *NavigationAgent::get_navigation_node() const {
+ return Object::cast_to(navigation);
+}
+
+void NavigationAgent::set_target_desired_distance(real_t p_dd) {
+ target_desired_distance = p_dd;
+}
+
+void NavigationAgent::set_radius(real_t p_radius) {
+ radius = p_radius;
+ NavigationServer::get_singleton()->agent_set_radius(agent, radius);
+}
+
+void NavigationAgent::set_agent_height_offset(real_t p_hh) {
+ navigation_height_offset = p_hh;
+}
+
+void NavigationAgent::set_ignore_y(bool p_ignore_y) {
+ ignore_y = p_ignore_y;
+ NavigationServer::get_singleton()->agent_set_ignore_y(agent, ignore_y);
+}
+
+void NavigationAgent::set_neighbor_dist(real_t p_dist) {
+ neighbor_dist = p_dist;
+ NavigationServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
+}
+
+void NavigationAgent::set_max_neighbors(int p_count) {
+ max_neighbors = p_count;
+ NavigationServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
+}
+
+void NavigationAgent::set_time_horizon(real_t p_time) {
+ time_horizon = p_time;
+ NavigationServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
+}
+
+void NavigationAgent::set_max_speed(real_t p_max_speed) {
+ max_speed = p_max_speed;
+ NavigationServer::get_singleton()->agent_set_max_speed(agent, max_speed);
+}
+
+void NavigationAgent::set_path_max_distance(real_t p_pmd) {
+ path_max_distance = p_pmd;
+}
+
+real_t NavigationAgent::get_path_max_distance() {
+ return path_max_distance;
+}
+
+void NavigationAgent::set_target_location(Vector3 p_location) {
+ target_location = p_location;
+ navigation_path.clear();
+ target_reached = false;
+ navigation_finished = false;
+}
+
+Vector3 NavigationAgent::get_target_location() const {
+ return target_location;
+}
+
+Vector3 NavigationAgent::get_next_location() {
+ update_navigation();
+ if (navigation_path.size() == 0) {
+ ERR_FAIL_COND_V(agent_parent == NULL, Vector3());
+ return agent_parent->get_global_transform().origin;
+ } else {
+ return navigation_path[nav_path_index] - Vector3(0, navigation_height_offset, 0);
+ }
+}
+
+real_t NavigationAgent::distance_to_target() const {
+ ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
+ return agent_parent->get_global_transform().origin.distance_to(target_location);
+}
+
+bool NavigationAgent::is_target_reached() const {
+ return target_reached;
+}
+
+bool NavigationAgent::is_target_reachable() {
+ return target_desired_distance >= get_final_location().distance_to(target_location);
+}
+
+bool NavigationAgent::is_navigation_finished() {
+ update_navigation();
+ return navigation_finished;
+}
+
+Vector3 NavigationAgent::get_final_location() {
+ update_navigation();
+ if (navigation_path.size() == 0) {
+ return Vector3();
+ }
+ return navigation_path[navigation_path.size() - 1];
+}
+
+void NavigationAgent::set_velocity(Vector3 p_velocity) {
+ target_velocity = p_velocity;
+ NavigationServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
+ NavigationServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
+ velocity_submitted = true;
+}
+
+void NavigationAgent::_avoidance_done(Vector3 p_new_velocity) {
+ prev_safe_velocity = p_new_velocity;
+
+ if (!velocity_submitted) {
+ target_velocity = Vector3();
+ return;
+ }
+ velocity_submitted = false;
+
+ emit_signal("velocity_computed", p_new_velocity);
+}
+
+String NavigationAgent::get_configuration_warning() const {
+ if (!Object::cast_to(get_parent())) {
+ return TTR("The NavigationAgent can be used only under a spatial node.");
+ }
+
+ return String();
+}
+
+void NavigationAgent::update_navigation() {
+
+ if (agent_parent == NULL) return;
+ if (navigation == NULL) return;
+ if (update_frame_id == Engine::get_singleton()->get_physics_frames()) return;
+
+ update_frame_id = Engine::get_singleton()->get_physics_frames();
+
+ Vector3 o = agent_parent->get_global_transform().origin;
+
+ bool reload_path = false;
+
+ if (NavigationServer::get_singleton()->agent_is_map_changed(agent)) {
+ reload_path = true;
+ } else if (navigation_path.size() == 0) {
+ reload_path = true;
+ } else {
+ // Check if too far from the navigation path
+ if (nav_path_index > 0) {
+ Vector3 segment[2];
+ segment[0] = navigation_path[nav_path_index - 1];
+ segment[1] = navigation_path[nav_path_index];
+ segment[0].y -= navigation_height_offset;
+ segment[1].y -= navigation_height_offset;
+ Vector3 p = Geometry::get_closest_point_to_segment(o, segment);
+ if (o.distance_to(p) >= path_max_distance) {
+ // To faraway, reload path
+ reload_path = true;
+ }
+ }
+ }
+
+ if (reload_path) {
+ navigation_path = NavigationServer::get_singleton()->map_get_path(navigation->get_rid(), o, target_location, true);
+ navigation_finished = false;
+ nav_path_index = 0;
+ emit_signal("path_changed");
+ }
+
+ if (navigation_path.size() == 0)
+ return;
+
+ // Check if we can advance the navigation path
+ if (navigation_finished == false) {
+ // Advances to the next far away location.
+ while (o.distance_to(navigation_path[nav_path_index] - Vector3(0, navigation_height_offset, 0)) < target_desired_distance) {
+ nav_path_index += 1;
+ if (nav_path_index == navigation_path.size()) {
+ nav_path_index -= 1;
+ navigation_finished = true;
+ emit_signal("navigation_finished");
+ break;
+ }
+ }
+ }
+}
diff --git a/scene/3d/navigation_agent.h b/scene/3d/navigation_agent.h
new file mode 100644
index 0000000000..8f181dafe1
--- /dev/null
+++ b/scene/3d/navigation_agent.h
@@ -0,0 +1,162 @@
+/*************************************************************************/
+/* navigation_agent.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md) */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAVIGATION_AGENT_H
+#define NAVIGATION_AGENT_H
+
+#include "core/vector.h"
+#include "scene/main/node.h"
+
+class Spatial;
+class Navigation;
+
+class NavigationAgent : public Node {
+ GDCLASS(NavigationAgent, Node);
+
+ Spatial *agent_parent;
+ Navigation *navigation;
+
+ RID agent;
+
+ real_t target_desired_distance;
+ real_t radius;
+ real_t navigation_height_offset;
+ bool ignore_y;
+ real_t neighbor_dist;
+ int max_neighbors;
+ real_t time_horizon;
+ real_t max_speed;
+
+ real_t path_max_distance;
+
+ Vector3 target_location;
+ Vector navigation_path;
+ int nav_path_index;
+ bool velocity_submitted;
+ Vector3 prev_safe_velocity;
+ /// The submitted target velocity
+ Vector3 target_velocity;
+ bool target_reached;
+ bool navigation_finished;
+ // No initialized on purpose
+ uint32_t update_frame_id;
+
+protected:
+ static void _bind_methods();
+ void _notification(int p_what);
+
+public:
+ NavigationAgent();
+ virtual ~NavigationAgent();
+
+ void set_navigation(Navigation *p_nav);
+ const Navigation *get_navigation() const {
+ return navigation;
+ }
+
+ void set_navigation_node(Node *p_nav);
+ Node *get_navigation_node() const;
+
+ RID get_rid() const {
+ return agent;
+ }
+
+ void set_target_desired_distance(real_t p_dd);
+ real_t get_target_desired_distance() const {
+ return target_desired_distance;
+ }
+
+ void set_radius(real_t p_radius);
+ real_t get_radius() const {
+ return radius;
+ }
+
+ void set_agent_height_offset(real_t p_hh);
+ real_t get_agent_height_offset() const {
+ return navigation_height_offset;
+ }
+
+ void set_ignore_y(bool p_ignore_y);
+ bool get_ignore_y() const {
+ return ignore_y;
+ }
+
+ void set_neighbor_dist(real_t p_dist);
+ real_t get_neighbor_dist() const {
+ return neighbor_dist;
+ }
+
+ void set_max_neighbors(int p_count);
+ int get_max_neighbors() const {
+ return max_neighbors;
+ }
+
+ void set_time_horizon(real_t p_time);
+ real_t get_time_horizon() const {
+ return time_horizon;
+ }
+
+ void set_max_speed(real_t p_max_speed);
+ real_t get_max_speed() const {
+ return max_speed;
+ }
+
+ void set_path_max_distance(real_t p_pmd);
+ real_t get_path_max_distance();
+
+ void set_target_location(Vector3 p_location);
+ Vector3 get_target_location() const;
+
+ Vector3 get_next_location();
+
+ Vector get_nav_path() const {
+ return navigation_path;
+ }
+
+ int get_nav_path_index() const {
+ return nav_path_index;
+ }
+
+ real_t distance_to_target() const;
+ bool is_target_reached() const;
+ bool is_target_reachable();
+ bool is_navigation_finished();
+ Vector3 get_final_location();
+
+ void set_velocity(Vector3 p_velocity);
+ void _avoidance_done(Vector3 p_new_velocity);
+
+ virtual String get_configuration_warning() const;
+
+private:
+ void update_navigation();
+};
+
+#endif
diff --git a/scene/3d/navigation_mesh_instance.cpp b/scene/3d/navigation_mesh_instance.cpp
new file mode 100644
index 0000000000..ef59767078
--- /dev/null
+++ b/scene/3d/navigation_mesh_instance.cpp
@@ -0,0 +1,258 @@
+/*************************************************************************/
+/* navigation_mesh.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#include "navigation_mesh_instance.h"
+#include "core/os/thread.h"
+#include "mesh_instance.h"
+#include "navigation.h"
+#include "servers/navigation_server.h"
+
+void NavigationMeshInstance::set_enabled(bool p_enabled) {
+
+ if (enabled == p_enabled)
+ return;
+ enabled = p_enabled;
+
+ if (!is_inside_tree())
+ return;
+
+ if (!enabled) {
+
+ NavigationServer::get_singleton()->region_set_map(region, RID());
+ } else {
+
+ if (navigation) {
+
+ NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
+ }
+ }
+
+ if (debug_view) {
+ MeshInstance *dm = Object::cast_to(debug_view);
+ if (is_enabled()) {
+ dm->set_material_override(get_tree()->get_debug_navigation_material());
+ } else {
+ dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
+ }
+ }
+
+ update_gizmo();
+}
+
+bool NavigationMeshInstance::is_enabled() const {
+
+ return enabled;
+}
+
+/////////////////////////////
+
+void NavigationMeshInstance::_notification(int p_what) {
+
+ switch (p_what) {
+ case NOTIFICATION_ENTER_TREE: {
+
+ Spatial *c = this;
+ while (c) {
+
+ navigation = Object::cast_to(c);
+ if (navigation) {
+
+ if (enabled) {
+
+ NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
+ }
+ break;
+ }
+
+ c = c->get_parent_spatial();
+ }
+
+ if (navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
+
+ MeshInstance *dm = memnew(MeshInstance);
+ dm->set_mesh(navmesh->get_debug_mesh());
+ if (is_enabled()) {
+ dm->set_material_override(get_tree()->get_debug_navigation_material());
+ } else {
+ dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
+ }
+ add_child(dm);
+ debug_view = dm;
+ }
+
+ } break;
+ case NOTIFICATION_TRANSFORM_CHANGED: {
+
+ NavigationServer::get_singleton()->region_set_transform(region, get_global_transform());
+
+ } break;
+ case NOTIFICATION_EXIT_TREE: {
+
+ if (navigation) {
+
+ NavigationServer::get_singleton()->region_set_map(region, RID());
+ }
+
+ if (debug_view) {
+ debug_view->queue_delete();
+ debug_view = NULL;
+ }
+ navigation = NULL;
+ } break;
+ }
+}
+
+void NavigationMeshInstance::set_navigation_mesh(const Ref &p_navmesh) {
+
+ if (p_navmesh == navmesh)
+ return;
+
+ if (navmesh.is_valid()) {
+ navmesh->remove_change_receptor(this);
+ }
+
+ navmesh = p_navmesh;
+
+ if (navmesh.is_valid()) {
+ navmesh->add_change_receptor(this);
+ }
+
+ NavigationServer::get_singleton()->region_set_navmesh(region, p_navmesh);
+
+ if (debug_view && navmesh.is_valid()) {
+ Object::cast_to(debug_view)->set_mesh(navmesh->get_debug_mesh());
+ }
+
+ emit_signal("navigation_mesh_changed");
+
+ update_gizmo();
+ update_configuration_warning();
+}
+
+Ref NavigationMeshInstance::get_navigation_mesh() const {
+
+ return navmesh;
+}
+
+struct BakeThreadsArgs {
+ NavigationMeshInstance *nav_mesh_instance;
+};
+
+void _bake_navigation_mesh(void *p_user_data) {
+ BakeThreadsArgs *args = static_cast(p_user_data);
+
+ if (args->nav_mesh_instance->get_navigation_mesh().is_valid()) {
+ Ref nav_mesh = args->nav_mesh_instance->get_navigation_mesh()->duplicate();
+
+ NavigationServer::get_singleton()->region_bake_navmesh(nav_mesh, args->nav_mesh_instance);
+ args->nav_mesh_instance->call_deferred("_bake_finished", nav_mesh);
+ memdelete(args);
+ } else {
+
+ ERR_PRINT("Can't bake the navigation mesh if the `NavigationMesh` resource doesn't exist");
+ args->nav_mesh_instance->call_deferred("_bake_finished", Ref());
+ memdelete(args);
+ }
+}
+
+void NavigationMeshInstance::bake_navigation_mesh() {
+ ERR_FAIL_COND(bake_thread != NULL);
+
+ BakeThreadsArgs *args = memnew(BakeThreadsArgs);
+ args->nav_mesh_instance = this;
+
+ bake_thread = Thread::create(_bake_navigation_mesh, args);
+ ERR_FAIL_COND(bake_thread == NULL);
+}
+
+void NavigationMeshInstance::_bake_finished(Ref p_nav_mesh) {
+ set_navigation_mesh(p_nav_mesh);
+ bake_thread = NULL;
+}
+
+String NavigationMeshInstance::get_configuration_warning() const {
+
+ if (!is_visible_in_tree() || !is_inside_tree())
+ return String();
+
+ if (!navmesh.is_valid()) {
+ return TTR("A NavigationMesh resource must be set or created for this node to work.");
+ }
+ const Spatial *c = this;
+ while (c) {
+
+ if (Object::cast_to(c))
+ return String();
+
+ c = Object::cast_to(c->get_parent());
+ }
+
+ return TTR("NavigationMeshInstance must be a child or grandchild to a Navigation node. It only provides navigation data.");
+}
+
+void NavigationMeshInstance::_bind_methods() {
+
+ ClassDB::bind_method(D_METHOD("set_navigation_mesh", "navmesh"), &NavigationMeshInstance::set_navigation_mesh);
+ ClassDB::bind_method(D_METHOD("get_navigation_mesh"), &NavigationMeshInstance::get_navigation_mesh);
+
+ ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &NavigationMeshInstance::set_enabled);
+ ClassDB::bind_method(D_METHOD("is_enabled"), &NavigationMeshInstance::is_enabled);
+
+ ClassDB::bind_method(D_METHOD("bake_navigation_mesh"), &NavigationMeshInstance::bake_navigation_mesh);
+ ClassDB::bind_method(D_METHOD("_bake_finished", "nav_mesh"), &NavigationMeshInstance::_bake_finished);
+
+ ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "navmesh", PROPERTY_HINT_RESOURCE_TYPE, "NavigationMesh"), "set_navigation_mesh", "get_navigation_mesh");
+ ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled");
+
+ ADD_SIGNAL(MethodInfo("navigation_mesh_changed"));
+ ADD_SIGNAL(MethodInfo("bake_finished"));
+}
+
+void NavigationMeshInstance::_changed_callback(Object *p_changed, const char *p_prop) {
+ update_gizmo();
+ update_configuration_warning();
+}
+
+NavigationMeshInstance::NavigationMeshInstance() {
+
+ enabled = true;
+ set_notify_transform(true);
+ region = NavigationServer::get_singleton()->region_create();
+
+ navigation = NULL;
+ debug_view = NULL;
+ bake_thread = NULL;
+}
+
+NavigationMeshInstance::~NavigationMeshInstance() {
+ if (navmesh.is_valid())
+ navmesh->remove_change_receptor(this);
+ NavigationServer::get_singleton()->free(region);
+}
diff --git a/scene/3d/navigation_mesh_instance.h b/scene/3d/navigation_mesh_instance.h
new file mode 100644
index 0000000000..0f23c55cc7
--- /dev/null
+++ b/scene/3d/navigation_mesh_instance.h
@@ -0,0 +1,75 @@
+/*************************************************************************/
+/* navigation_mesh.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the */
+/* "Software"), to deal in the Software without restriction, including */
+/* without limitation the rights to use, copy, modify, merge, publish, */
+/* distribute, sublicense, and/or sell copies of the Software, and to */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions: */
+/* */
+/* The above copyright notice and this permission notice shall be */
+/* included in all copies or substantial portions of the Software. */
+/* */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
+/*************************************************************************/
+
+#ifndef NAVIGATION_MESH_INSTANCE_H
+#define NAVIGATION_MESH_INSTANCE_H
+
+#include "scene/3d/spatial.h"
+#include "scene/resources/mesh.h"
+#include "scene/resources/navigation_mesh.h"
+
+class Navigation;
+
+class NavigationMeshInstance : public Spatial {
+
+ GDCLASS(NavigationMeshInstance, Spatial);
+
+ bool enabled;
+ RID region;
+ Ref navmesh;
+
+ Navigation *navigation;
+ Node *debug_view;
+ Thread *bake_thread;
+
+protected:
+ void _notification(int p_what);
+ static void _bind_methods();
+ void _changed_callback(Object *p_changed, const char *p_prop);
+
+public:
+ void set_enabled(bool p_enabled);
+ bool is_enabled() const;
+
+ void set_navigation_mesh(const Ref