From a6ac305f967a272c35f984b046517629a401b688 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Tue, 10 Jan 2023 07:14:16 +0100 Subject: [PATCH] Rework Navigation Avoidance Rework Navigation Avoidance. --- SConstruct | 3 +- core/core_constants.cpp | 1 + core/object/object.h | 1 + doc/classes/@GlobalScope.xml | 53 +- doc/classes/NavigationAgent2D.xml | 65 +- doc/classes/NavigationAgent3D.xml | 79 ++- doc/classes/NavigationObstacle2D.xml | 53 +- doc/classes/NavigationObstacle3D.xml | 60 +- doc/classes/NavigationRegion2D.xml | 22 + doc/classes/NavigationServer2D.xml | 119 +++- doc/classes/NavigationServer3D.xml | 158 ++++- doc/classes/ProjectSettings.xml | 129 ++++ editor/editor_properties.cpp | 12 +- editor/editor_properties.h | 1 + editor/editor_run.cpp | 5 + editor/plugins/debugger_editor_plugin.cpp | 13 + editor/plugins/debugger_editor_plugin.h | 1 + .../navigation_obstacle_2d_editor_plugin.cpp | 66 ++ .../navigation_obstacle_2d_editor_plugin.h | 61 ++ .../navigation_obstacle_3d_editor_plugin.cpp | 599 ++++++++++++++++++ .../navigation_obstacle_3d_editor_plugin.h | 117 ++++ editor/register_editor_types.cpp | 4 + main/main.cpp | 15 + modules/gdscript/doc_classes/@GDScript.xml | 10 + modules/gdscript/gdscript_parser.cpp | 1 + modules/gridmap/grid_map.cpp | 4 +- modules/navigation/SCsub | 29 +- .../navigation/godot_navigation_server.cpp | 194 +++++- modules/navigation/godot_navigation_server.h | 27 +- modules/navigation/nav_agent.cpp | 290 ++++++++- modules/navigation/nav_agent.h | 113 +++- modules/navigation/nav_map.cpp | 242 ++++++- modules/navigation/nav_map.h | 46 +- modules/navigation/nav_obstacle.cpp | 86 +++ modules/navigation/nav_obstacle.h | 78 +++ scene/2d/navigation_agent_2d.cpp | 222 +++++-- scene/2d/navigation_agent_2d.h | 114 ++-- scene/2d/navigation_obstacle_2d.cpp | 353 ++++++----- scene/2d/navigation_obstacle_2d.h | 66 +- scene/2d/navigation_region_2d.cpp | 145 ++++- scene/2d/navigation_region_2d.h | 17 + scene/3d/navigation_agent_3d.cpp | 282 +++++++-- scene/3d/navigation_agent_3d.h | 136 ++-- scene/3d/navigation_link_3d.cpp | 2 +- scene/3d/navigation_obstacle_3d.cpp | 588 ++++++++++++----- scene/3d/navigation_obstacle_3d.h | 78 ++- scene/3d/navigation_region_3d.cpp | 8 +- scene/main/scene_tree.cpp | 8 + scene/main/scene_tree.h | 4 + scene/register_scene_types.cpp | 4 + servers/navigation_server_2d.cpp | 148 ++++- servers/navigation_server_2d.h | 66 +- servers/navigation_server_3d.cpp | 276 +++++++- servers/navigation_server_3d.h | 120 +++- servers/navigation_server_3d_dummy.h | 24 +- thirdparty/README.md | 17 +- .../rvo2/patches/rvo2-godot-changes.patch | 282 --------- thirdparty/rvo2/rvo2_2d/Agent2d.cpp | 594 +++++++++++++++++ thirdparty/rvo2/rvo2_2d/Agent2d.h | 160 +++++ thirdparty/rvo2/rvo2_2d/Definitions.h | 109 ++++ thirdparty/rvo2/rvo2_2d/KdTree2d.cpp | 357 +++++++++++ thirdparty/rvo2/rvo2_2d/KdTree2d.h | 203 ++++++ thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp | 38 ++ thirdparty/rvo2/rvo2_2d/Obstacle2d.h | 72 +++ thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp | 363 +++++++++++ thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h | 592 +++++++++++++++++ thirdparty/rvo2/rvo2_2d/Vector2.h | 346 ++++++++++ .../rvo2/{Agent.cpp => rvo2_3d/Agent3d.cpp} | 114 ++-- .../rvo2/{Agent.h => rvo2_3d/Agent3d.h} | 59 +- thirdparty/rvo2/{ => rvo2_3d}/Definitions.h | 2 +- .../rvo2/{KdTree.cpp => rvo2_3d/KdTree3d.cpp} | 17 +- .../rvo2/{KdTree.h => rvo2_3d/KdTree3d.h} | 30 +- thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp | 274 ++++++++ thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h | 324 ++++++++++ thirdparty/rvo2/{ => rvo2_3d}/Vector3.h | 38 +- 75 files changed, 8211 insertions(+), 1198 deletions(-) create mode 100644 editor/plugins/navigation_obstacle_2d_editor_plugin.cpp create mode 100644 editor/plugins/navigation_obstacle_2d_editor_plugin.h create mode 100644 editor/plugins/navigation_obstacle_3d_editor_plugin.cpp create mode 100644 editor/plugins/navigation_obstacle_3d_editor_plugin.h create mode 100644 modules/navigation/nav_obstacle.cpp create mode 100644 modules/navigation/nav_obstacle.h delete mode 100644 thirdparty/rvo2/patches/rvo2-godot-changes.patch create mode 100644 thirdparty/rvo2/rvo2_2d/Agent2d.cpp create mode 100644 thirdparty/rvo2/rvo2_2d/Agent2d.h create mode 100644 thirdparty/rvo2/rvo2_2d/Definitions.h create mode 100644 thirdparty/rvo2/rvo2_2d/KdTree2d.cpp create mode 100644 thirdparty/rvo2/rvo2_2d/KdTree2d.h create mode 100644 thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp create mode 100644 thirdparty/rvo2/rvo2_2d/Obstacle2d.h create mode 100644 thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp create mode 100644 thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h create mode 100644 thirdparty/rvo2/rvo2_2d/Vector2.h rename thirdparty/rvo2/{Agent.cpp => rvo2_3d/Agent3d.cpp} (84%) rename thirdparty/rvo2/{Agent.h => rvo2_3d/Agent3d.h} (63%) rename thirdparty/rvo2/{ => rvo2_3d}/Definitions.h (98%) rename thirdparty/rvo2/{KdTree.cpp => rvo2_3d/KdTree3d.cpp} (92%) rename thirdparty/rvo2/{KdTree.h => rvo2_3d/KdTree3d.h} (81%) create mode 100644 thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp create mode 100644 thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h rename thirdparty/rvo2/{ => rvo2_3d}/Vector3.h (92%) diff --git a/SConstruct b/SConstruct index e5421b7887a..f4d27a21344 100644 --- a/SConstruct +++ b/SConstruct @@ -241,7 +241,8 @@ opts.Add(BoolVariable("builtin_miniupnpc", "Use the built-in miniupnpc library", 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_recastnavigation", "Use the built-in Recast navigation library", True)) -opts.Add(BoolVariable("builtin_rvo2", "Use the built-in RVO2 library", True)) +opts.Add(BoolVariable("builtin_rvo2_2d", "Use the built-in RVO2 2D library", True)) +opts.Add(BoolVariable("builtin_rvo2_3d", "Use the built-in RVO2 3D 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/core/core_constants.cpp b/core/core_constants.cpp index d88dda66090..15c09359681 100644 --- a/core/core_constants.cpp +++ b/core/core_constants.cpp @@ -645,6 +645,7 @@ void register_global_constants() { BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_LAYERS_3D_RENDER); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_LAYERS_3D_PHYSICS); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_LAYERS_3D_NAVIGATION); + BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_LAYERS_AVOIDANCE); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_FILE); BIND_CORE_ENUM_CONSTANT(PROPERTY_HINT_DIR); diff --git a/core/object/object.h b/core/object/object.h index ed2c6254174..00bdec2228b 100644 --- a/core/object/object.h +++ b/core/object/object.h @@ -61,6 +61,7 @@ enum PropertyHint { PROPERTY_HINT_LAYERS_3D_RENDER, PROPERTY_HINT_LAYERS_3D_PHYSICS, PROPERTY_HINT_LAYERS_3D_NAVIGATION, + PROPERTY_HINT_LAYERS_AVOIDANCE, PROPERTY_HINT_FILE, ///< a file path must be passed, hint_text (optionally) is a filter "*.png,*.wav,*.doc," PROPERTY_HINT_DIR, ///< a directory path must be passed PROPERTY_HINT_GLOBAL_FILE, ///< a file path must be passed, hint_text (optionally) is a filter "*.png,*.wav,*.doc," diff --git a/doc/classes/@GlobalScope.xml b/doc/classes/@GlobalScope.xml index a0b623936a6..11d343e7fbd 100644 --- a/doc/classes/@GlobalScope.xml +++ b/doc/classes/@GlobalScope.xml @@ -2695,36 +2695,39 @@ Hints that an [int] property is a bitmask using the optionally named 3D navigation layers. - + + Hints that an integer property is a bitmask using the optionally named avoidance layers. + + Hints that a [String] property is a path to a file. Editing it will show a file dialog for picking the path. The hint string can be a set of filters with wildcards like [code]"*.png,*.jpg"[/code]. - + Hints that a [String] property is a path to a directory. Editing it will show a file dialog for picking the path. - + Hints that a [String] property is an absolute path to a file outside the project folder. Editing it will show a file dialog for picking the path. The hint string can be a set of filters with wildcards, like [code]"*.png,*.jpg"[/code]. - + Hints that a [String] property is an absolute path to a directory outside the project folder. Editing it will show a file dialog for picking the path. - + Hints that a property is an instance of a [Resource]-derived type, optionally specified via the hint string (e.g. [code]"Texture2D"[/code]). Editing it will show a popup menu of valid resource types to instantiate. - + Hints that a [String] property is text with line breaks. Editing it will show a text input field where line breaks can be typed. - + Hints that a [String] property is an [Expression]. - + Hints that a [String] property should show a placeholder text on its input field, if empty. The hint string is the placeholder text to use. - + Hints that a [Color] property should be edited without affecting its transparency ([member Color.a] is not editable). - + - + Hint that a property represents a particular type. If a property is [constant TYPE_STRING], allows to set a type from the create dialog. If you need to create an [Array] to contain elements of a specific type, the [code]hint_string[/code] must encode nested types using [code]":"[/code] and [code]"/"[/code] for specifying [Resource] types. For instance: [codeblock] hint_string = "%s:" % [TYPE_INT] # Array of integers. @@ -2734,37 +2737,37 @@ [/codeblock] [b]Note:[/b] The final colon is required for properly detecting built-in types. - + - + - + - + - + - + - + - + - + Hints that a string property is a locale code. Editing it will show a locale dialog for picking language and country. - + Hints that a dictionary property is string translation map. Dictionary keys are locale codes and, values are translated strings. - + - + Hints that a quaternion property should disable the temporary euler editor. - + Hints that a string property is a password, and every character is replaced with the secret character. - + Represents the size of the [enum PropertyHint] enum. diff --git a/doc/classes/NavigationAgent2D.xml b/doc/classes/NavigationAgent2D.xml index 1c68058d2f7..e401450e425 100644 --- a/doc/classes/NavigationAgent2D.xml +++ b/doc/classes/NavigationAgent2D.xml @@ -5,7 +5,7 @@ 2D Agent that is used in navigation to reach a position while avoiding static and dynamic obstacles. The dynamic obstacles are avoided using RVO collision avoidance. The agent needs navigation data to work correctly. [NavigationAgent2D] is physics safe. - [b]Note:[/b] After setting [member target_position] it is required to use the [method get_next_path_position] function once every physics frame to update the internal path logic of the NavigationAgent. The returned vector position from this function should be used as the next movement position for the agent's parent Node. + [b]Note:[/b] After [member target_position] is set, the [method get_next_path_position] function must be used once every physics frame to update the internal path logic of the NavigationAgent. The returned position from this function should be used as the next movement position for the agent's parent node. $DOCS_URL/tutorials/navigation/navigation_using_navigationagents.html @@ -17,6 +17,20 @@ Returns the distance to the target position, using the agent's global position. The user must set [member target_position] in order for this to be accurate. + + + + + Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32. + + + + + + + Returns whether or not the specified mask of the [member avoidance_mask] bitmask is enabled, given a [param mask_number] between 1 and 32. + + @@ -38,7 +52,7 @@ - Returns the reachable final position 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 reachable final position of the current navigation path 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. @@ -75,7 +89,7 @@ - Returns true if [member target_position] is reachable. + Returns true if [member target_position] is reachable. The target position is set using [member target_position]. @@ -84,6 +98,22 @@ Returns true if [member target_position] is reached. It may not always be possible to reach the target position. It should always be possible to reach the final position though. See [method get_final_position]. + + + + + + Based on [param value], enables or disables the specified layer in the [member avoidance_layers] bitmask, given a [param layer_number] between 1 and 32. + + + + + + + + Based on [param value], enables or disables the specified mask in the [member avoidance_mask] bitmask, given a [param mask_number] between 1 and 32. + + @@ -99,17 +129,26 @@ Sets the [RID] of the navigation map this NavigationAgent node should use and also updates the [code]agent[/code] on the NavigationServer. - + - 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. + Replaces the internal velocity in the collision avoidance simulation with [param velocity]. When an agent is teleported to a new position this function should be used in the same frame. If called frequently this function can get agents stuck. - If [code]true[/code] the agent is registered for an RVO avoidance callback on the [NavigationServer2D]. When [method set_velocity] is used and the processing is completed a [code]safe_velocity[/code] Vector2 is received with a signal connection to [signal velocity_computed]. Avoidance processing with many registered agents has a significant performance cost and should only be enabled on agents that currently require it. + If [code]true[/code] the agent is registered for an RVO avoidance callback on the [NavigationServer2D]. When [member velocity] is used and the processing is completed a [code]safe_velocity[/code] Vector2 is received with a signal connection to [signal velocity_computed]. Avoidance processing with many registered agents has a significant performance cost and should only be enabled on agents that currently require it. + + + A bitfield determining the avoidance layers for this NavigationAgent. Other agent's with a matching bit on the [member avoidance_mask] will avoid this agent. + + + A bitfield determining what other avoidance agent's and obstacles this NavigationAgent will avoid when a bit matches at least one of their [member avoidance_layers]. + + + The agent does not adjust the velocity for other agents that would match the [member avoidance_mask] but have a lower [member avoidance_priority]. This in turn makes the other agents with lower priority adjust their velocities even more to avoid collision with this agent. If [code]true[/code] shows debug visuals for this agent. @@ -161,10 +200,16 @@ The distance threshold before the final target point is considered to be reached. This will allow an agent to not have to hit the point of the final target exactly, but only the area. If this value is set to low the NavigationAgent will be stuck in a repath loop cause it will constantly overshoot or undershoot the distance to the final target point on each physics frame update. - The user-defined target position. Setting this property will clear the current navigation path. + If set a new navigation path from the current agent position to the [member target_position] is requested from the NavigationServer. - - The minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithm, 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 minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithm, 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. A too high value will slow down agents movement considerably. Must be positive. + + + The minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithm, are safe with respect to static avoidance obstacles. The larger the number, the sooner the agent will respond to static avoidance obstacles, but less freedom in choosing its velocities. A too high value will slow down agents movement considerably. Must be positive. + + + Sets the new wanted velocity for the agent. The avoidance simulation will try to fulfil this velocity if possible but will modify it to avoid collision with other agent's and obstacles. When an agent is teleported to a new position use [method set_velocity_forced] as well to reset the internal simulation velocity. @@ -199,7 +244,7 @@ - Notifies when the collision avoidance velocity is calculated. Emitted at the end of the physics frame in which [method set_velocity] is called. Only emitted when [member avoidance_enabled] is true. + Notifies when the collision avoidance velocity is calculated. Emitted when [member velocity] is set. Only emitted when [member avoidance_enabled] is true. diff --git a/doc/classes/NavigationAgent3D.xml b/doc/classes/NavigationAgent3D.xml index 6fdea5431db..00ef894378b 100644 --- a/doc/classes/NavigationAgent3D.xml +++ b/doc/classes/NavigationAgent3D.xml @@ -5,7 +5,7 @@ 3D Agent that is used in navigation to reach a position while avoiding static and dynamic obstacles. The dynamic obstacles are avoided using RVO collision avoidance. The agent needs navigation data to work correctly. [NavigationAgent3D] is physics safe. - [b]Note:[/b] After setting [member target_position] it is required to use the [method get_next_path_position] function once every physics frame to update the internal path logic of the NavigationAgent. The returned vector position from this function should be used as the next movement position for the agent's parent Node. + [b]Note:[/b] After [member target_position] is set, the [method get_next_path_position] function must be used once every physics frame to update the internal path logic of the NavigationAgent. The returned position from this function should be used as the next movement position for the agent's parent node. $DOCS_URL/tutorials/navigation/navigation_using_navigationagents.html @@ -17,6 +17,20 @@ Returns the distance to the target position, using the agent's global position. The user must set [member target_position] in order for this to be accurate. + + + + + Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32. + + + + + + + Returns whether or not the specified mask of the [member avoidance_mask] bitmask is enabled, given a [param mask_number] between 1 and 32. + + @@ -38,7 +52,7 @@ - Returns the reachable final position 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 reachable final position of the current navigation path in global coordinates. This position can change if the navigation path is altered in any way. Because of this, it would be best to check this each frame. @@ -75,7 +89,7 @@ - Returns true if [member target_position] is reachable. + Returns true if [member target_position] is reachable. The target position is set using [member target_position]. @@ -84,6 +98,22 @@ Returns true if [member target_position] is reached. It may not always be possible to reach the target position. It should always be possible to reach the final position though. See [method get_final_position]. + + + + + + Based on [param value], enables or disables the specified layer in the [member avoidance_layers] bitmask, given a [param layer_number] between 1 and 32. + + + + + + + + Based on [param value], enables or disables the specified mask in the [member avoidance_mask] bitmask, given a [param mask_number] between 1 and 32. + + @@ -99,20 +129,26 @@ Sets the [RID] of the navigation map this NavigationAgent node should use and also updates the [code]agent[/code] on the NavigationServer. - + - 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. + Replaces the internal velocity in the collision avoidance simulation with [param velocity]. When an agent is teleported to a new position this function should be used in the same frame. If called frequently this function can get agents stuck. - - The NavigationAgent height offset is subtracted from the y-axis value of any vector path position for this NavigationAgent. The NavigationAgent height offset does not change or influence the navigation mesh or pathfinding query result. Additional navigation maps that use regions with navigation meshes that the developer baked with appropriate agent radius or height values are required to support different-sized agents. - - If [code]true[/code] the agent is registered for an RVO avoidance callback on the [NavigationServer3D]. When [method set_velocity] is used and the processing is completed a [code]safe_velocity[/code] Vector3 is received with a signal connection to [signal velocity_computed]. Avoidance processing with many registered agents has a significant performance cost and should only be enabled on agents that currently require it. + If [code]true[/code] the agent is registered for an RVO avoidance callback on the [NavigationServer3D]. When [member velocity] is set and the processing is completed a [code]safe_velocity[/code] Vector3 is received with a signal connection to [signal velocity_computed]. Avoidance processing with many registered agents has a significant performance cost and should only be enabled on agents that currently require it. + + + A bitfield determining the avoidance layers for this NavigationAgent. Other agent's with a matching bit on the [member avoidance_mask] will avoid this agent. + + + A bitfield determining what other avoidance agent's and obstacles this NavigationAgent will avoid when a bit matches at least one of their [member avoidance_layers]. + + + The agent does not adjust the velocity for other agents that would match the [member avoidance_mask] but have a lower [member avoidance_priority]. This in turn makes the other agents with lower priority adjust their velocities even more to avoid collision with this agent. If [code]true[/code] shows debug visuals for this agent. @@ -126,8 +162,8 @@ If [code]true[/code] uses the defined [member debug_path_custom_color] for this agent instead of global color. - - Ignores collisions on the Y axis. Must be true to move on a horizontal plane. + + The height of the avoidance agent. Agent's will ignore other agents or obstacles that are above or below their current position + height in 2D avoidance. Does nothing in 3D avoidance which uses radius spheres alone. The maximum number of neighbors for the agent to consider. @@ -144,6 +180,9 @@ The distance threshold before a path point is considered to be reached. This will allow an agent to not have to hit a path point on the path exactly, but in the area. If this value is set to high the NavigationAgent will skip points on the path which can lead to leaving the navigation mesh. If this value is set to low the NavigationAgent will be stuck in a repath loop cause it will constantly overshoot or undershoot the distance to the next point on each physics frame update. + + The height offset is subtracted from the y-axis value of any vector path position for this NavigationAgent. The NavigationAgent height offset does not change or influence the navigation mesh or pathfinding query result. Additional navigation maps that use regions with navigation meshes that the developer baked with appropriate agent radius or height values are required to support different-sized agents. + The maximum distance the agent is allowed away from the ideal path to the final position. This can happen due to trying to avoid collisions. When the maximum distance is exceeded, it recalculates the ideal path. @@ -164,10 +203,20 @@ The distance threshold before the final target point is considered to be reached. This will allow an agent to not have to hit the point of the final target exactly, but only the area. If this value is set to low the NavigationAgent will be stuck in a repath loop cause it will constantly overshoot or undershoot the distance to the final target point on each physics frame update. - The user-defined target position. Setting this property will clear the current navigation path. + If set a new navigation path from the current agent position to the [member target_position] is requested from the NavigationServer. - - The minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithm, 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 minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithm, 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. A too high value will slow down agents movement considerably. Must be positive. + + + The minimal amount of time for which this agent's velocities, that are computed with the collision avoidance algorithm, are safe with respect to static avoidance obstacles. The larger the number, the sooner the agent will respond to static avoidance obstacles, but less freedom in choosing its velocities. A too high value will slow down agents movement considerably. Must be positive. + + + If [code]true[/code] the agent calculates avoidance velocities in 3D for the xyz-axis, e.g. for games that take place in air, unterwater or space. The 3D using agent only avoids other 3D avoidance using agent's. The 3D using agent only reacts to radius based avoidance obstacles. The 3D using agent ignores any vertices based obstacles. The 3D using agent only avoids other 3D using agent's. + If [code]false[/code] the agent calculates avoidance velocities in 2D along the xz-axis ignoring the y-axis. The 2D using agent only avoids other 2D avoidance using agent's. The 2D using agent reacts to radius avoidance obstacles. The 2D using agent reacts to vertices based avoidance obstacles. The 2D using agent only avoids other 2D using agent's. 2D using agents will ignore other 2D using agents or obstacles that are below their current position or above their current position including [member height] in 2D avoidance. + + + Sets the new wanted velocity for the agent. The avoidance simulation will try to fulfil this velocity if possible but will modify it to avoid collision with other agent's and obstacles. When an agent is teleported to a new position use [method set_velocity_forced] as well to reset the internal simulation velocity. @@ -202,7 +251,7 @@ - Notifies when the collision avoidance velocity is calculated. Emitted at the end of the physics frame in which [method set_velocity] is called. Only emitted when [member avoidance_enabled] is true. + Notifies when the collision avoidance velocity is calculated. Emitted when [member velocity] is set. Only emitted when [member avoidance_enabled] is true. diff --git a/doc/classes/NavigationObstacle2D.xml b/doc/classes/NavigationObstacle2D.xml index 06334e3dc77..e365d1aec55 100644 --- a/doc/classes/NavigationObstacle2D.xml +++ b/doc/classes/NavigationObstacle2D.xml @@ -1,42 +1,71 @@ - + - 2D Obstacle used in navigation for collision avoidance. + 2D Obstacle used in navigation to constrain avoidance controlled agents outside or inside an area. - 2D Obstacle used in navigation for collision avoidance. The obstacle needs navigation data to work correctly. [NavigationObstacle2D] is physics safe. - Obstacles [b]don't[/b] change the resulting path from the pathfinding, they only affect the navigation agent movement in a radius. Therefore, using obstacles for the static walls in your level won't work because those walls don't exist in the pathfinding. The navigation agent will be pushed in a semi-random direction away while moving inside that radius. Obstacles are intended as a last resort option for constantly moving objects that cannot be (re)baked to a navigation mesh efficiently. + 2D Obstacle used in navigation to constrain avoidance controlled agents outside or inside an area. The obstacle needs a navigation map and outline vertices defined to work correctly. + If the obstacle's vertices are winded in clockwise order, avoidance agents will be pushed in by the obstacle, otherwise, avoidance agents will be pushed out. Outlines must not cross or overlap. + Obstacles are [b]not[/b] a replacement for a (re)baked navigation mesh. Obstacles [b]don't[/b] change the resulting path from the pathfinding, obstacles only affect the navigation avoidance agent movement by altering the suggested velocity of the avoidance agent. + Obstacles using vertices can warp to a new position but should not moved every frame as each move requires a rebuild of the avoidance map. $DOCS_URL/tutorials/navigation/navigation_using_navigationobstacles.html + + + + Returns the [RID] of this agent on the [NavigationServer2D]. This [RID] is used for the moving avoidance "obstacle" component (using a fake avoidance agent) which size is defined by [member radius] and velocity set by using [member velocity]. + + + + + + + Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32. + + - Returns the [RID] of the navigation map for this NavigationObstacle node. This function returns always the map set on the NavigationObstacle node and not the map of the abstract agent on the NavigationServer. If the agent map is changed directly with the NavigationServer API the NavigationObstacle node will not be aware of the map change. Use [method set_navigation_map] to change the navigation map for the NavigationObstacle and also update the agent on the NavigationServer. + Returns the [RID] of the navigation map for this NavigationObstacle node. This function returns always the map set on the NavigationObstacle node and not the map of the abstract obstacle on the NavigationServer. If the obstacle map is changed directly with the NavigationServer API the NavigationObstacle node will not be aware of the map change. Use [method set_navigation_map] to change the navigation map for the NavigationObstacle and also update the obstacle on the NavigationServer. - + - Returns the [RID] of this obstacle on the [NavigationServer2D]. + Returns the [RID] of this obstacle on the [NavigationServer2D]. This [RID] is used for the static avoidance obstacle component which shape is defined by [member vertices]. + + + + + + + + Based on [param value], enables or disables the specified layer in the [member avoidance_layers] bitmask, given a [param layer_number] between 1 and 32. - Sets the [RID] of the navigation map this NavigationObstacle node should use and also updates the [code]agent[/code] on the NavigationServer. + Sets the [RID] of the navigation map this NavigationObstacle node should use and also updates the [code]obstacle[/code] on the NavigationServer. - - Enables radius estimation algorithm which uses parent's collision shapes to determine the obstacle radius. + + A bitfield determining the avoidance layers for this obstacle. Agent's with a matching bit on the their avoidance mask will avoid this obstacle. - - The radius of the agent. Used only if [member estimate_radius] is set to false. + + Sets the avoidance radius for the obstacle. + + + Sets the wanted velocity for the obstacle so other agent's can better predict the obstacle if it is moved with a velocity regularly (every frame) instead of warped to a new position. Does only affect avoidance for the obstacles [member radius]. Does nothing for the obstacles static vertices. + + + The outline vertices of the obstacle. If the vertices are winded in clockwise order agents will be pushed in by the obstacle, else they will be pushed out. Outlines can not be crossed or overlap. Should the vertices using obstacle be warped to a new position agent's can not predict this movement and may get trapped inside the obstacle. diff --git a/doc/classes/NavigationObstacle3D.xml b/doc/classes/NavigationObstacle3D.xml index 2a2642fe8c7..eb76d86fc01 100644 --- a/doc/classes/NavigationObstacle3D.xml +++ b/doc/classes/NavigationObstacle3D.xml @@ -1,42 +1,78 @@ - + - 3D Obstacle used in navigation for collision avoidance. + 3D Obstacle used in navigation to constrain avoidance controlled agents outside or inside an area. - 3D Obstacle used in navigation for collision avoidance. The obstacle needs navigation data to work correctly. [NavigationObstacle3D] is physics safe. - Obstacles [b]don't[/b] change the resulting path from the pathfinding, they only affect the navigation agent movement in a radius. Therefore, using obstacles for the static walls in your level won't work because those walls don't exist in the pathfinding. The navigation agent will be pushed in a semi-random direction away while moving inside that radius. Obstacles are intended as a last resort option for constantly moving objects that cannot be (re)baked to a navigation mesh efficiently. + 3D Obstacle used in navigation to constrain avoidance controlled agents outside or inside an area. The obstacle needs a navigation map and outline vertices defined to work correctly. + If the obstacle's vertices are winded in clockwise order, avoidance agents will be pushed in by the obstacle, otherwise, avoidance agents will be pushed out. Outlines must not cross or overlap. + Obstacles are [b]not[/b] a replacement for a (re)baked navigation mesh. Obstacles [b]don't[/b] change the resulting path from the pathfinding, obstacles only affect the navigation avoidance agent movement by altering the suggested velocity of the avoidance agent. + Obstacles using vertices can warp to a new position but should not moved every frame as each move requires a rebuild of the avoidance map. $DOCS_URL/tutorials/navigation/navigation_using_navigationobstacles.html + + + + Returns the [RID] of this agent on the [NavigationServer3D]. This [RID] is used for the moving avoidance "obstacle" component (using a fake avoidance agent) which size is defined by [member radius] and velocity set by using [member velocity]. + + + + + + + Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32. + + - Returns the [RID] of the navigation map for this NavigationObstacle node. This function returns always the map set on the NavigationObstacle node and not the map of the abstract agent on the NavigationServer. If the agent map is changed directly with the NavigationServer API the NavigationObstacle node will not be aware of the map change. Use [method set_navigation_map] to change the navigation map for the NavigationObstacle and also update the agent on the NavigationServer. + Returns the [RID] of the navigation map for this NavigationObstacle node. This function returns always the map set on the NavigationObstacle node and not the map of the abstract obstacle on the NavigationServer. If the obstacle map is changed directly with the NavigationServer API the NavigationObstacle node will not be aware of the map change. Use [method set_navigation_map] to change the navigation map for the NavigationObstacle and also update the obstacle on the NavigationServer. - + - Returns the [RID] of this obstacle on the [NavigationServer3D]. + Returns the [RID] of this obstacle on the [NavigationServer3D]. This [RID] is used for the static avoidance obstacle component which shape is defined by [member vertices]. + + + + + + + + Based on [param value], enables or disables the specified layer in the [member avoidance_layers] bitmask, given a [param layer_number] between 1 and 32. - Sets the [RID] of the navigation map this NavigationObstacle node should use and also updates the [code]agent[/code] on the NavigationServer. + Sets the [RID] of the navigation map this NavigationObstacle node should use and also updates the [code]obstacle[/code] on the NavigationServer. - - Enables radius estimation algorithm which uses parent's collision shapes to determine the obstacle radius. + + A bitfield determining the avoidance layers for this obstacle. Agent's with a matching bit on the their avoidance mask will avoid this obstacle. - - The radius of the agent. Used only if [member estimate_radius] is set to false. + + Sets the obstacle height used in 2D avoidance. 2D avoidance using agent's ignore obstacles that are below or above them. + + + Sets the avoidance radius for the obstacle. + + + If [code]true[/code] the obstacle affects 3D avoidance using agent's with obstacle [member radius]. + If [code]false[/code] the obstacle affects 2D avoidance using agent's with both obstacle [member vertices] as well as obstacle [member radius]. + + + Sets the wanted velocity for the obstacle so other agent's can better predict the obstacle if it is moved with a velocity regularly (every frame) instead of warped to a new position. Does only affect avoidance for the obstacles [member radius]. Does nothing for the obstacles static vertices. + + + The outline vertices of the obstacle. If the vertices are winded in clockwise order agents will be pushed in by the obstacle, else they will be pushed out. Outlines can not be crossed or overlap. Should the vertices using obstacle be warped to a new position agent's can not predict this movement and may get trapped inside the obstacle. diff --git a/doc/classes/NavigationRegion2D.xml b/doc/classes/NavigationRegion2D.xml index f05fd74685d..bb5992f1122 100644 --- a/doc/classes/NavigationRegion2D.xml +++ b/doc/classes/NavigationRegion2D.xml @@ -16,6 +16,13 @@ $DOCS_URL/tutorials/navigation/navigation_using_navigationregions.html + + + + + Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32. + + @@ -29,6 +36,14 @@ Returns the [RID] of this region on the [NavigationServer2D]. Combined with [method NavigationServer2D.map_get_closest_point_owner] can be used to identify the [NavigationRegion2D] closest to a point on the merged navigation map. + + + + + + Based on [param value], enables or disables the specified layer in the [member avoidance_layers] bitmask, given a [param layer_number] between 1 and 32. + + @@ -39,6 +54,13 @@ + + A bitfield determining all avoidance layers for the avoidance constrain. + + + If [code]true[/code] constraints avoidance agent's with an avoidance mask bit that matches with a bit of the [member avoidance_layers] to the navigation polygon. Due to each navigation polygon outline creating an obstacle and each polygon edge creating an avoidance line constrain keep the navigation polygon shape as simple as possible for performance. + [b]Experimental:[/b] This is an experimental feature and should not be used in production as agent's can get stuck on the navigation polygon corners and edges especially at high frame rate. + Determines if the [NavigationRegion2D] is enabled or disabled. diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index e96f3dc7e45..f9e6a694524 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -24,6 +24,13 @@ Creates the agent. + + + + + Return [code]true[/code] if the specified [param agent] uses avoidance. + + @@ -38,13 +45,46 @@ Returns true if the map got changed the previous frame. - + - Sets the callback that gets called after each avoidance processing step for the [param agent]. The calculated [code]safe_velocity[/code] will be passed as the first parameter just before the physics calculations. - [b]Note:[/b] Created callbacks are always processed independently of the SceneTree state as long as the agent is on a navigation map and not freed. To disable the dispatch of a callback from an agent use [method agent_set_callback] again with an empty [Callable]. + Sets the callback [Callable] that gets called after each avoidance processing step for the [param agent]. The calculated [code]safe_velocity[/code] will be dispatched with a signal to the object just before the physics calculations. + [b]Note:[/b] Created callbacks are always processed independently of the SceneTree state as long as the agent is on a navigation map and not freed. To disable the dispatch of a callback from an agent use [method agent_set_avoidance_callback] again with an empty [Callable]. + + + + + + + + If [param enabled] is [code]true[/code] the specified [param agent] uses avoidance. + + + + + + + + Set the agent's [code]avoidance_layers[/code] bitmask. + + + + + + + + Set the agent's [code]avoidance_mask[/code] bitmask. + + + + + + + + Set the agent's [code]avoidance_priority[/code] with a [param priority] between 0.0 (lowest priority) to 1.0 (highest priority). + The specified [param agent] does not adjust the velocity for other agents that would match the [code]avoidance_mask[/code] but have a lower [code] avoidance_priority[/code]. This in turn makes the other agents with lower priority adjust their velocities even more to avoid collision with this agent. @@ -95,20 +135,20 @@ 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. A too high value will slow down agents movement considerably. Must be positive. - + - + - 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. + The minimal amount of time for which the agent's velocities that are computed by the simulation are safe with respect to static avoidance obstacles. The larger this number, the sooner this agent will respond to the presence of static avoidance obstacles, but the less freedom this agent has in choosing its velocities. A too high value will slow down agents movement considerably. Must be positive. @@ -116,7 +156,15 @@ - Sets the current velocity of the agent. + Sets [param velocity] as the new wanted velocity for the specified [param agent]. The avoidance simulation will try to fulfil this velocity if possible but will modify it to avoid collision with other agent's and obstacles. When an agent is teleported to a new position far away use [method agent_set_velocity_forced] instead to reset the internal velocity state. + + + + + + + + Replaces the internal velocity in the collision avoidance simulation with [param velocity] for the specified [param agent]. When an agent is teleported to a new position far away this function should be used in the same frame. If called frequently this function can get agents stuck. @@ -331,6 +379,13 @@ Returns all navigation link [RID]s that are currently assigned to the requested navigation [param map]. + + + + + Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [code]map[/code]. + + @@ -388,6 +443,50 @@ Set the map's link connection radius used to connect links to navigation polygons. + + + + Creates a new navigation obstacle. + + + + + + + Returns the navigation map [RID] the requested [param obstacle] is currently assigned to. + + + + + + + + + + + + + + + Sets the navigation map [RID] for the obstacle. + + + + + + + + Sets the position of the obstacle in world space. + + + + + + + + Sets the outline vertices for the obstacle. If the vertices are winded in clockwise order agents will be pushed in by the obstacle, else they will be pushed out. + + diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml index 5b79355b61a..3fe07937154 100644 --- a/doc/classes/NavigationServer3D.xml +++ b/doc/classes/NavigationServer3D.xml @@ -24,6 +24,13 @@ Creates the agent. + + + + + Returns [code]true[/code] if the provided [param agent] has avoidance enabled. + + @@ -31,6 +38,13 @@ Returns the navigation map [RID] the requested [param agent] is currently assigned to. + + + + + Returns [code]true[/code] if the provided [param agent] uses avoidance in 3D space Vector3(x,y,z) instead of horizontal 2D Vector2(x,y) / Vector3(x,0.0,z). + + @@ -38,13 +52,54 @@ Returns true if the map got changed the previous frame. - + - Sets the callback that gets called after each avoidance processing step for the [param agent]. The calculated [code]safe_velocity[/code] will be passed as the first parameter just before the physics calculations. - [b]Note:[/b] Created callbacks are always processed independently of the SceneTree state as long as the agent is on a navigation map and not freed. To disable the dispatch of a callback from an agent use [method agent_set_callback] again with an empty [Callable]. + Sets the callback [Callable] that gets called after each avoidance processing step for the [param agent]. The calculated [code]safe_velocity[/code] will be dispatched with a signal to the object just before the physics calculations. + [b]Note:[/b] Created callbacks are always processed independently of the SceneTree state as long as the agent is on a navigation map and not freed. To disable the dispatch of a callback from an agent use [method agent_set_avoidance_callback] again with an empty [Callable]. + + + + + + + + If [param enabled] the provided [param agent] calculates avoidance. + + + + + + + + Set the agent's [code]avoidance_layers[/code] bitmask. + + + + + + + + Set the agent's [code]avoidance_mask[/code] bitmask. + + + + + + + + Set the agent's [code]avoidance_priority[/code] with a [param priority] between 0.0 (lowest priority) to 1.0 (highest priority). + The specified [param agent] does not adjust the velocity for other agents that would match the [code]avoidance_mask[/code] but have a lower [code] avoidance_priority[/code]. This in turn makes the other agents with lower priority adjust their velocities even more to avoid collision with this agent. + + + + + + + + Updates the provided [param agent] [param height]. @@ -95,20 +150,30 @@ 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. A too high value will slow down agents movement considerably. Must be positive. - + - + - 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. + The minimal amount of time for which the agent's velocities that are computed by the simulation are safe with respect to static avoidance obstacles. The larger this number, the sooner this agent will respond to the presence of static avoidance obstacles, but the less freedom this agent has in choosing its velocities. A too high value will slow down agents movement considerably. Must be positive. + + + + + + + + Sets if the agent uses the 2D avoidance or the 3D avoidance while avoidance is enabled. + If [code]true[/code] the agent calculates avoidance velocities in 3D for the xyz-axis, e.g. for games that take place in air, unterwater or space. The 3D using agent only avoids other 3D avoidance using agent's. The 3D using agent only reacts to radius based avoidance obstacles. The 3D using agent ignores any vertices based obstacles. The 3D using agent only avoids other 3D using agent's. + If [code]false[/code] the agent calculates avoidance velocities in 2D along the xz-axis ignoring the y-axis. The 2D using agent only avoids other 2D avoidance using agent's. The 2D using agent reacts to radius avoidance obstacles. The 2D using agent reacts to vertices based avoidance obstacles. The 2D using agent only avoids other 2D using agent's. 2D using agents will ignore other 2D using agents or obstacles that are below their current position or above their current position including the agents height in 2D avoidance. @@ -116,7 +181,15 @@ - Sets the current velocity of the agent. + Sets [param velocity] as the new wanted velocity for the specified [param agent]. The avoidance simulation will try to fulfil this velocity if possible but will modify it to avoid collision with other agent's and obstacles. When an agent is teleported to a new position use [method agent_set_velocity_forced] as well to reset the internal simulation velocity. + + + + + + + + Replaces the internal velocity in the collision avoidance simulation with [param velocity] for the specified [param agent]. When an agent is teleported to a new position this function should be used in the same frame. If called frequently this function can get agents stuck. @@ -356,6 +429,13 @@ Returns all navigation link [RID]s that are currently assigned to the requested navigation [param map]. + + + + + Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [code]map[/code]. + + @@ -428,6 +508,59 @@ Sets the map up direction. + + + + Creates a new obstacle. + + + + + + + Returns the navigation map [RID] the requested [param obstacle] is currently assigned to. + + + + + + + + Set the obstacles's [code]avoidance_layers[/code] bitmask. + + + + + + + + Sets the [param height] for the [param obstacle]. In 3D agents will ignore obstacles that are above or below them while using 2D avoidance. + + + + + + + + Assigns the [param obstacle] to a navigation map. + + + + + + + + Updates the [param position] in world space for the [param obstacle]. + + + + + + + + Sets the outline vertices for the obstacle. If the vertices are winded in clockwise order agents will be pushed in by the obstacle, else they will be pushed out. + + @@ -590,6 +723,11 @@ + + + Emitted when avoidance debug settings are changed. Only available in debug builds. + + diff --git a/doc/classes/ProjectSettings.xml b/doc/classes/ProjectSettings.xml index ef335bd526d..2c54e364c7f 100644 --- a/doc/classes/ProjectSettings.xml +++ b/doc/classes/ProjectSettings.xml @@ -579,6 +579,33 @@ When set to [code]true[/code], produces a warning when a varying is never used. + + Color of the avoidance agents radius, visible when "Visible Avoidance" is enabled in the Debug menu. + + + If enabled, displays avoidance agents radius when "Visible Avoidance" is enabled in the Debug menu. + + + If enabled, displays avoidance obstacles radius when "Visible Avoidance" is enabled in the Debug menu. + + + If enabled, displays static avoidance obstacles when "Visible Avoidance" is enabled in the Debug menu. + + + Color of the avoidance obstacles radius, visible when "Visible Avoidance" is enabled in the Debug menu. + + + Color of the static avoidance obstacles edges when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu. + + + Color of the static avoidance obstacles edges when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu. + + + Color of the static avoidance obstacles faces when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu. + + + Color of the static avoidance obstacles faces when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu. + Color of the contact points between collision shapes, visible when "Visible Collision Shapes" is enabled in the Debug menu. @@ -1777,6 +1804,102 @@ Optional name for the 3D render layer 20. If left empty, the layer will display as "Layer 20". + + Optional name for the navigation avoidance layer 1. If left empty, the layer will display as "Layer 1". + + + Optional name for the navigation avoidance layer 2. If left empty, the layer will display as "Layer 2". + + + Optional name for the navigation avoidance layer 3. If left empty, the layer will display as "Layer 3". + + + Optional name for the navigation avoidance layer 4. If left empty, the layer will display as "Layer 4". + + + Optional name for the navigation avoidance layer 5. If left empty, the layer will display as "Layer 5". + + + Optional name for the navigation avoidance layer 6. If left empty, the layer will display as "Layer 6". + + + Optional name for the navigation avoidance layer 7. If left empty, the layer will display as "Layer 7". + + + Optional name for the navigation avoidance layer 8. If left empty, the layer will display as "Layer 8". + + + Optional name for the navigation avoidance layer 9. If left empty, the layer will display as "Layer 9". + + + Optional name for the navigation avoidance layer 10. If left empty, the layer will display as "Layer 10". + + + Optional name for the navigation avoidance layer 11. If left empty, the layer will display as "Layer 11". + + + Optional name for the navigation avoidance layer 12. If left empty, the layer will display as "Layer 12". + + + Optional name for the navigation avoidance layer 13. If left empty, the layer will display as "Layer 13". + + + Optional name for the navigation avoidance layer 14. If left empty, the layer will display as "Layer 14". + + + Optional name for the navigation avoidance layer 15. If left empty, the layer will display as "Layer 15". + + + Optional name for the navigation avoidance layer 16. If left empty, the layer will display as "Layer 16". + + + Optional name for the navigation avoidance layer 17. If left empty, the layer will display as "Layer 17". + + + Optional name for the navigation avoidance layer 18. If left empty, the layer will display as "Layer 18". + + + Optional name for the navigation avoidance layer 19. If left empty, the layer will display as "Layer 19". + + + Optional name for the navigation avoidance layer 20. If left empty, the layer will display as "Layer 20". + + + Optional name for the navigation avoidance layer 21. If left empty, the layer will display as "Layer 21". + + + Optional name for the navigation avoidance layer 22. If left empty, the layer will display as "Layer 22". + + + Optional name for the navigation avoidance layer 23. If left empty, the layer will display as "Layer 23". + + + Optional name for the navigation avoidance layer 24. If left empty, the layer will display as "Layer 24". + + + Optional name for the navigation avoidance layer 25. If left empty, the layer will display as "Layer 25". + + + Optional name for the navigation avoidance layer 26. If left empty, the layer will display as "Layer 26". + + + Optional name for the navigation avoidance layer 27. If left empty, the layer will display as "Layer 27". + + + Optional name for the navigation avoidance layer 28. If left empty, the layer will display as "Layer 28". + + + Optional name for the navigation avoidance layer 29. If left empty, the layer will display as "Layer 29". + + + Optional name for the navigation avoidance layer 30. If left empty, the layer will display as "Layer 30". + + + Optional name for the navigation avoidance layer 31. If left empty, the layer will display as "Layer 31". + + + Optional name for the navigation avoidance layer 32. If left empty, the layer will display as "Layer 32". + Godot uses a message queue to defer some function calls. If you run out of space on it (you will see an error), you can increase the size here. @@ -1801,6 +1924,12 @@ Default link connection radius for 3D navigation maps. See [method NavigationServer3D.map_set_link_connection_radius]. + + If enabled and avoidance calculations use multiple threads the threads run with high priority. + + + If enabled the avoidance calculations use multiple threads. + Maximum number of characters allowed to send as output from the debugger. Over this value, content is dropped. This helps not to stall the debugger connection. diff --git a/editor/editor_properties.cpp b/editor/editor_properties.cpp index 18c5d4ba518..9bba030c41a 100644 --- a/editor/editor_properties.cpp +++ b/editor/editor_properties.cpp @@ -1155,6 +1155,12 @@ void EditorPropertyLayers::setup(LayerType p_layer_type) { layer_group_size = 4; layer_count = 32; } break; + + case LAYER_AVOIDANCE: { + basename = "layer_names/avoidance"; + layer_group_size = 4; + layer_count = 32; + } break; } Vector names; @@ -4284,7 +4290,8 @@ EditorProperty *EditorInspectorDefaultPlugin::get_editor_for_property(Object *p_ p_hint == PROPERTY_HINT_LAYERS_2D_NAVIGATION || p_hint == PROPERTY_HINT_LAYERS_3D_PHYSICS || p_hint == PROPERTY_HINT_LAYERS_3D_RENDER || - p_hint == PROPERTY_HINT_LAYERS_3D_NAVIGATION) { + p_hint == PROPERTY_HINT_LAYERS_3D_NAVIGATION || + p_hint == PROPERTY_HINT_LAYERS_AVOIDANCE) { EditorPropertyLayers::LayerType lt = EditorPropertyLayers::LAYER_RENDER_2D; switch (p_hint) { case PROPERTY_HINT_LAYERS_2D_RENDER: @@ -4305,6 +4312,9 @@ EditorProperty *EditorInspectorDefaultPlugin::get_editor_for_property(Object *p_ case PROPERTY_HINT_LAYERS_3D_NAVIGATION: lt = EditorPropertyLayers::LAYER_NAVIGATION_3D; break; + case PROPERTY_HINT_LAYERS_AVOIDANCE: + lt = EditorPropertyLayers::LAYER_AVOIDANCE; + break; default: { } //compiler could be smarter here and realize this can't happen } diff --git a/editor/editor_properties.h b/editor/editor_properties.h index 0d54025c7b3..015c65b4c6b 100644 --- a/editor/editor_properties.h +++ b/editor/editor_properties.h @@ -308,6 +308,7 @@ public: LAYER_PHYSICS_3D, LAYER_RENDER_3D, LAYER_NAVIGATION_3D, + LAYER_AVOIDANCE, }; private: diff --git a/editor/editor_run.cpp b/editor/editor_run.cpp index b090638c37f..25c25c3f7c2 100644 --- a/editor/editor_run.cpp +++ b/editor/editor_run.cpp @@ -70,6 +70,7 @@ Error EditorRun::run(const String &p_scene, const String &p_write_movie) { bool debug_collisions = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_collisions", false); bool debug_paths = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_paths", false); bool debug_navigation = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_navigation", false); + bool debug_avoidance = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_avoidance", false); if (debug_collisions) { args.push_back("--debug-collisions"); } @@ -82,6 +83,10 @@ Error EditorRun::run(const String &p_scene, const String &p_write_movie) { args.push_back("--debug-navigation"); } + if (debug_avoidance) { + args.push_back("--debug-avoidance"); + } + if (p_write_movie != "") { args.push_back("--write-movie"); args.push_back(p_write_movie); diff --git a/editor/plugins/debugger_editor_plugin.cpp b/editor/plugins/debugger_editor_plugin.cpp index aecf3d295ca..3068ad3f932 100644 --- a/editor/plugins/debugger_editor_plugin.cpp +++ b/editor/plugins/debugger_editor_plugin.cpp @@ -77,6 +77,9 @@ DebuggerEditorPlugin::DebuggerEditorPlugin(PopupMenu *p_debug_menu) { debug_menu->add_check_shortcut(ED_SHORTCUT("editor/visible_navigation", TTR("Visible Navigation")), RUN_DEBUG_NAVIGATION); debug_menu->set_item_tooltip(-1, TTR("When this option is enabled, navigation meshes and polygons will be visible in the running project.")); + debug_menu->add_check_shortcut(ED_SHORTCUT("editor/visible_avoidance", TTR("Visible Avoidance")), RUN_DEBUG_AVOIDANCE); + debug_menu->set_item_tooltip(-1, + TTR("When this option is enabled, avoidance objects shapes, radius and velocities will be visible in the running project.")); debug_menu->add_separator(); debug_menu->add_check_shortcut(ED_SHORTCUT("editor/sync_scene_changes", TTR("Synchronize Scene Changes")), RUN_LIVE_DEBUG); debug_menu->set_item_tooltip(-1, @@ -167,6 +170,12 @@ void DebuggerEditorPlugin::_menu_option(int p_option) { debug_menu->set_item_checked(debug_menu->get_item_index(RUN_DEBUG_NAVIGATION), !ischecked); EditorSettings::get_singleton()->set_project_metadata("debug_options", "run_debug_navigation", !ischecked); + } break; + case RUN_DEBUG_AVOIDANCE: { + bool ischecked = debug_menu->is_item_checked(debug_menu->get_item_index(RUN_DEBUG_AVOIDANCE)); + debug_menu->set_item_checked(debug_menu->get_item_index(RUN_DEBUG_AVOIDANCE), !ischecked); + EditorSettings::get_singleton()->set_project_metadata("debug_options", "run_debug_avoidance", !ischecked); + } break; case RUN_RELOAD_SCRIPTS: { bool ischecked = debug_menu->is_item_checked(debug_menu->get_item_index(RUN_RELOAD_SCRIPTS)); @@ -205,6 +214,7 @@ void DebuggerEditorPlugin::_update_debug_options() { bool check_debug_collisions = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_collisions", false); bool check_debug_paths = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_paths", false); bool check_debug_navigation = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_navigation", false); + bool check_debug_avoidance = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_debug_avoidance", false); bool check_live_debug = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_live_debug", true); bool check_reload_scripts = EditorSettings::get_singleton()->get_project_metadata("debug_options", "run_reload_scripts", true); bool check_server_keep_open = EditorSettings::get_singleton()->get_project_metadata("debug_options", "server_keep_open", false); @@ -225,6 +235,9 @@ void DebuggerEditorPlugin::_update_debug_options() { if (check_debug_navigation) { _menu_option(RUN_DEBUG_NAVIGATION); } + if (check_debug_avoidance) { + _menu_option(RUN_DEBUG_AVOIDANCE); + } if (check_live_debug) { _menu_option(RUN_LIVE_DEBUG); } diff --git a/editor/plugins/debugger_editor_plugin.h b/editor/plugins/debugger_editor_plugin.h index 6e2d2c02ee6..eb8da7ca8e5 100644 --- a/editor/plugins/debugger_editor_plugin.h +++ b/editor/plugins/debugger_editor_plugin.h @@ -51,6 +51,7 @@ private: RUN_DEBUG_COLLISIONS, RUN_DEBUG_PATHS, RUN_DEBUG_NAVIGATION, + RUN_DEBUG_AVOIDANCE, RUN_DEPLOY_REMOTE_DEBUG, RUN_RELOAD_SCRIPTS, SERVER_KEEP_OPEN, diff --git a/editor/plugins/navigation_obstacle_2d_editor_plugin.cpp b/editor/plugins/navigation_obstacle_2d_editor_plugin.cpp new file mode 100644 index 00000000000..0cbc7119824 --- /dev/null +++ b/editor/plugins/navigation_obstacle_2d_editor_plugin.cpp @@ -0,0 +1,66 @@ +/**************************************************************************/ +/* navigation_obstacle_2d_editor_plugin.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "navigation_obstacle_2d_editor_plugin.h" + +#include "editor/editor_node.h" +#include "editor/editor_undo_redo_manager.h" + +Node2D *NavigationObstacle2DEditor::_get_node() const { + return node; +} + +void NavigationObstacle2DEditor::_set_node(Node *p_polygon) { + node = Object::cast_to(p_polygon); +} + +void NavigationObstacle2DEditor::_action_add_polygon(const Variant &p_polygon) { + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->add_do_method(node, "set_vertices", p_polygon); + undo_redo->add_undo_method(node, "set_vertices", node->get_vertices()); +} + +void NavigationObstacle2DEditor::_action_remove_polygon(int p_idx) { + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->add_do_method(node, "set_vertices", Variant(Vector())); + undo_redo->add_undo_method(node, "set_vertices", node->get_vertices()); +} + +void NavigationObstacle2DEditor::_action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) { + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->add_do_method(node, "set_vertices", p_polygon); + undo_redo->add_undo_method(node, "set_vertices", node->get_vertices()); +} + +NavigationObstacle2DEditor::NavigationObstacle2DEditor() {} + +NavigationObstacle2DEditorPlugin::NavigationObstacle2DEditorPlugin() : + AbstractPolygon2DEditorPlugin(memnew(NavigationObstacle2DEditor), "NavigationObstacle2D") { +} diff --git a/editor/plugins/navigation_obstacle_2d_editor_plugin.h b/editor/plugins/navigation_obstacle_2d_editor_plugin.h new file mode 100644 index 00000000000..5a2206b8df2 --- /dev/null +++ b/editor/plugins/navigation_obstacle_2d_editor_plugin.h @@ -0,0 +1,61 @@ +/**************************************************************************/ +/* navigation_obstacle_2d_editor_plugin.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef NAVIGATION_OBSTACLE_2D_EDITOR_PLUGIN_H +#define NAVIGATION_OBSTACLE_2D_EDITOR_PLUGIN_H + +#include "editor/plugins/abstract_polygon_2d_editor.h" +#include "scene/2d/navigation_obstacle_2d.h" + +class NavigationObstacle2DEditor : public AbstractPolygon2DEditor { + GDCLASS(NavigationObstacle2DEditor, AbstractPolygon2DEditor); + + NavigationObstacle2D *node = nullptr; + +protected: + virtual Node2D *_get_node() const override; + virtual void _set_node(Node *p_polygon) override; + + virtual void _action_add_polygon(const Variant &p_polygon) override; + virtual void _action_remove_polygon(int p_idx) override; + virtual void _action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) override; + +public: + NavigationObstacle2DEditor(); +}; + +class NavigationObstacle2DEditorPlugin : public AbstractPolygon2DEditorPlugin { + GDCLASS(NavigationObstacle2DEditorPlugin, AbstractPolygon2DEditorPlugin); + +public: + NavigationObstacle2DEditorPlugin(); +}; + +#endif // NAVIGATION_OBSTACLE_2D_EDITOR_PLUGIN_H diff --git a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp new file mode 100644 index 00000000000..2e39d6f67ce --- /dev/null +++ b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp @@ -0,0 +1,599 @@ +/**************************************************************************/ +/* navigation_obstacle_3d_editor_plugin.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "navigation_obstacle_3d_editor_plugin.h" + +#include "canvas_item_editor_plugin.h" +#include "core/core_string_names.h" +#include "core/input/input.h" +#include "core/io/file_access.h" +#include "core/math/geometry_2d.h" +#include "core/os/keyboard.h" +#include "editor/editor_node.h" +#include "editor/editor_settings.h" +#include "editor/editor_undo_redo_manager.h" +#include "node_3d_editor_plugin.h" +#include "scene/3d/camera_3d.h" +#include "scene/gui/separator.h" + +void NavigationObstacle3DEditor::_notification(int p_what) { + switch (p_what) { + case NOTIFICATION_READY: { + button_create->set_icon(get_theme_icon(SNAME("Edit"), SNAME("EditorIcons"))); + button_edit->set_icon(get_theme_icon(SNAME("MovePoint"), SNAME("EditorIcons"))); + button_edit->set_pressed(true); + get_tree()->connect("node_removed", callable_mp(this, &NavigationObstacle3DEditor::_node_removed)); + + } break; + } +} + +void NavigationObstacle3DEditor::_node_removed(Node *p_node) { + if (p_node == obstacle_node) { + obstacle_node = nullptr; + if (point_lines_meshinstance->get_parent() == p_node) { + p_node->remove_child(point_lines_meshinstance); + } + hide(); + } +} + +void NavigationObstacle3DEditor::_menu_option(int p_option) { + switch (p_option) { + case MODE_CREATE: { + mode = MODE_CREATE; + button_create->set_pressed(true); + button_edit->set_pressed(false); + } break; + case MODE_EDIT: { + mode = MODE_EDIT; + button_create->set_pressed(false); + button_edit->set_pressed(true); + } break; + } +} + +void NavigationObstacle3DEditor::_wip_close() { + ERR_FAIL_COND_MSG(!obstacle_node, "Edited NavigationObstacle3D is not valid."); + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->create_action(TTR("Set NavigationObstacle3D Vertices")); + undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices()); + + PackedVector3Array polygon_3d_vertices; + Vector triangulated_polygon_2d_indices = Geometry2D::triangulate_polygon(wip); + + if (!triangulated_polygon_2d_indices.is_empty()) { + polygon_3d_vertices.resize(wip.size()); + Vector3 *polygon_3d_vertices_ptr = polygon_3d_vertices.ptrw(); + for (int i = 0; i < wip.size(); i++) { + const Vector2 &vert = wip[i]; + polygon_3d_vertices_ptr[i] = Vector3(vert.x, 0.0, vert.y); + } + } + + undo_redo->add_do_method(obstacle_node, "set_vertices", polygon_3d_vertices); + undo_redo->add_do_method(this, "_polygon_draw"); + undo_redo->add_undo_method(this, "_polygon_draw"); + wip.clear(); + wip_active = false; + mode = MODE_EDIT; + button_edit->set_pressed(true); + button_create->set_pressed(false); + edited_point = -1; + undo_redo->commit_action(); +} + +EditorPlugin::AfterGUIInput NavigationObstacle3DEditor::forward_3d_gui_input(Camera3D *p_camera, const Ref &p_event) { + if (!obstacle_node) { + return EditorPlugin::AFTER_GUI_INPUT_PASS; + } + + Transform3D gt = obstacle_node->get_global_transform(); + Transform3D gi = gt.affine_inverse(); + Plane p(Vector3(0.0, 1.0, 0.0), gt.origin); + + Ref mb = p_event; + + if (mb.is_valid()) { + Vector2 gpoint = mb->get_position(); + Vector3 ray_from = p_camera->project_ray_origin(gpoint); + Vector3 ray_dir = p_camera->project_ray_normal(gpoint); + + Vector3 spoint; + + if (!p.intersects_ray(ray_from, ray_dir, &spoint)) { + return EditorPlugin::AFTER_GUI_INPUT_PASS; + } + + spoint = gi.xform(spoint); + + Vector2 cpoint(spoint.x, spoint.z); + + //DO NOT snap here, it's confusing in 3D for adding points. + //Let the snap happen when the point is being moved, instead. + //cpoint = CanvasItemEditor::get_singleton()->snap_point(cpoint); + + PackedVector2Array poly = _get_polygon(); + + //first check if a point is to be added (segment split) + real_t grab_threshold = EDITOR_GET("editors/polygon_editor/point_grab_radius"); + + switch (mode) { + case MODE_CREATE: { + if (mb->get_button_index() == MouseButton::LEFT && mb->is_pressed()) { + if (!wip_active) { + wip.clear(); + wip.push_back(cpoint); + wip_active = true; + edited_point_pos = cpoint; + snap_ignore = false; + _polygon_draw(); + edited_point = 1; + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } else { + if (wip.size() > 1 && p_camera->unproject_position(gt.xform(Vector3(wip[0].x, 0.0, wip[0].y))).distance_to(gpoint) < grab_threshold) { + //wip closed + _wip_close(); + + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } else { + wip.push_back(cpoint); + edited_point = wip.size(); + snap_ignore = false; + _polygon_draw(); + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } + } + } else if (mb->get_button_index() == MouseButton::RIGHT && mb->is_pressed() && wip_active) { + _wip_close(); + } + + } break; + + case MODE_EDIT: { + if (mb->get_button_index() == MouseButton::LEFT) { + if (mb->is_pressed()) { + if (mb->is_ctrl_pressed()) { + if (poly.size() < 3) { + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->create_action(TTR("Edit Vertices")); + undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices()); + poly.push_back(cpoint); + undo_redo->add_do_method(this, "_polygon_draw"); + undo_redo->add_undo_method(this, "_polygon_draw"); + undo_redo->commit_action(); + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } + + //search edges + int closest_idx = -1; + Vector2 closest_pos; + real_t closest_dist = 1e10; + for (int i = 0; i < poly.size(); i++) { + Vector2 points[2] = { + p_camera->unproject_position(gt.xform(Vector3(poly[i].x, 0.0, poly[i].y))), + p_camera->unproject_position(gt.xform(Vector3(poly[(i + 1) % poly.size()].x, 0.0, poly[(i + 1) % poly.size()].y))) + }; + + Vector2 cp = Geometry2D::get_closest_point_to_segment(gpoint, points); + if (cp.distance_squared_to(points[0]) < CMP_EPSILON2 || cp.distance_squared_to(points[1]) < CMP_EPSILON2) { + continue; //not valid to reuse point + } + + real_t d = cp.distance_to(gpoint); + if (d < closest_dist && d < grab_threshold) { + closest_dist = d; + closest_pos = cp; + closest_idx = i; + } + } + + if (closest_idx >= 0) { + pre_move_edit = poly; + poly.insert(closest_idx + 1, cpoint); + edited_point = closest_idx + 1; + edited_point_pos = cpoint; + _set_polygon(poly); + _polygon_draw(); + snap_ignore = true; + + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } + } else { + //look for points to move + + int closest_idx = -1; + Vector2 closest_pos; + real_t closest_dist = 1e10; + for (int i = 0; i < poly.size(); i++) { + Vector2 cp = p_camera->unproject_position(gt.xform(Vector3(poly[i].x, 0.0, poly[i].y))); + + real_t d = cp.distance_to(gpoint); + if (d < closest_dist && d < grab_threshold) { + closest_dist = d; + closest_pos = cp; + closest_idx = i; + } + } + + if (closest_idx >= 0) { + pre_move_edit = poly; + edited_point = closest_idx; + edited_point_pos = poly[closest_idx]; + _polygon_draw(); + snap_ignore = false; + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } + } + } else { + snap_ignore = false; + + if (edited_point != -1) { + //apply + + ERR_FAIL_INDEX_V(edited_point, poly.size(), EditorPlugin::AFTER_GUI_INPUT_PASS); + poly.write[edited_point] = edited_point_pos; + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->create_action(TTR("Edit Poly")); + //undo_redo->add_do_method(obj, "set_polygon", poly); + //undo_redo->add_undo_method(obj, "set_polygon", pre_move_edit); + undo_redo->add_do_method(this, "_polygon_draw"); + undo_redo->add_undo_method(this, "_polygon_draw"); + undo_redo->commit_action(); + + edited_point = -1; + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } + } + } + if (mb->get_button_index() == MouseButton::RIGHT && mb->is_pressed() && edited_point == -1) { + int closest_idx = -1; + Vector2 closest_pos; + real_t closest_dist = 1e10; + for (int i = 0; i < poly.size(); i++) { + Vector2 cp = p_camera->unproject_position(gt.xform(Vector3(poly[i].x, 0.0, poly[i].y))); + + real_t d = cp.distance_to(gpoint); + if (d < closest_dist && d < grab_threshold) { + closest_dist = d; + closest_pos = cp; + closest_idx = i; + } + } + + if (closest_idx >= 0) { + EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton(); + undo_redo->create_action(TTR("Edit Poly (Remove Point)")); + //undo_redo->add_undo_method(obj, "set_polygon", poly); + poly.remove_at(closest_idx); + //undo_redo->add_do_method(obj, "set_polygon", poly); + undo_redo->add_do_method(this, "_polygon_draw"); + undo_redo->add_undo_method(this, "_polygon_draw"); + undo_redo->commit_action(); + return EditorPlugin::AFTER_GUI_INPUT_STOP; + } + } + + } break; + } + } + + Ref mm = p_event; + + if (mm.is_valid()) { + if (edited_point != -1 && (wip_active || mm->get_button_mask().has_flag(MouseButtonMask::LEFT))) { + Vector2 gpoint = mm->get_position(); + + Vector3 ray_from = p_camera->project_ray_origin(gpoint); + Vector3 ray_dir = p_camera->project_ray_normal(gpoint); + + Vector3 spoint; + + if (!p.intersects_ray(ray_from, ray_dir, &spoint)) { + return EditorPlugin::AFTER_GUI_INPUT_PASS; + } + + spoint = gi.xform(spoint); + + Vector2 cpoint(spoint.x, spoint.z); + + if (snap_ignore && !Input::get_singleton()->is_key_pressed(Key::CTRL)) { + snap_ignore = false; + } + + if (!snap_ignore && Node3DEditor::get_singleton()->is_snap_enabled()) { + cpoint = cpoint.snapped(Vector2( + Node3DEditor::get_singleton()->get_translate_snap(), + Node3DEditor::get_singleton()->get_translate_snap())); + } + edited_point_pos = cpoint; + + _polygon_draw(); + } + } + + return EditorPlugin::AFTER_GUI_INPUT_PASS; +} + +PackedVector2Array NavigationObstacle3DEditor::_get_polygon() { + ERR_FAIL_COND_V_MSG(!obstacle_node, PackedVector2Array(), "Edited object is not valid."); + return PackedVector2Array(obstacle_node->call("get_polygon")); +} + +void NavigationObstacle3DEditor::_set_polygon(PackedVector2Array p_poly) { + ERR_FAIL_COND_MSG(!obstacle_node, "Edited object is not valid."); + obstacle_node->call("set_polygon", p_poly); +} + +void NavigationObstacle3DEditor::_polygon_draw() { + if (!obstacle_node) { + return; + } + + PackedVector2Array poly; + PackedVector3Array polygon_3d_vertices; + + if (wip_active) { + poly = wip; + } else { + poly = _get_polygon(); + } + polygon_3d_vertices.resize(poly.size()); + Vector3 *polygon_3d_vertices_ptr = polygon_3d_vertices.ptrw(); + + for (int i = 0; i < poly.size(); i++) { + const Vector2 &vert = poly[i]; + polygon_3d_vertices_ptr[i] = Vector3(vert.x, 0.0, vert.y); + } + + point_handle_mesh->clear_surfaces(); + point_lines_mesh->clear_surfaces(); + point_lines_meshinstance->set_material_override(line_material); + point_lines_mesh->surface_begin(Mesh::PRIMITIVE_LINES); + + Rect2 rect; + + for (int i = 0; i < poly.size(); i++) { + Vector2 p, p2; + if (i == edited_point) { + p = edited_point_pos; + } else { + p = poly[i]; + } + + if ((wip_active && i == poly.size() - 1) || (((i + 1) % poly.size()) == edited_point)) { + p2 = edited_point_pos; + } else { + p2 = poly[(i + 1) % poly.size()]; + } + + if (i == 0) { + rect.position = p; + } else { + rect.expand_to(p); + } + + Vector3 point = Vector3(p.x, 0.0, p.y); + Vector3 next_point = Vector3(p2.x, 0.0, p2.y); + + point_lines_mesh->surface_set_color(Color(1, 0.3, 0.1, 0.8)); + point_lines_mesh->surface_add_vertex(point); + point_lines_mesh->surface_set_color(Color(1, 0.3, 0.1, 0.8)); + point_lines_mesh->surface_add_vertex(next_point); + + //Color col=Color(1,0.3,0.1,0.8); + //vpc->draw_line(point,next_point,col,2); + //vpc->draw_texture(handle,point-handle->get_size()*0.5); + } + + rect = rect.grow(1); + + AABB r; + r.position.x = rect.position.x; + r.position.y = 0.0; + r.position.z = rect.position.y; + r.size.x = rect.size.x; + r.size.y = 0; + r.size.z = rect.size.y; + + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(0.3, 0, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(0.0, 0.3, 0)); + + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(r.size.x, 0, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(r.size.x, 0, 0) - Vector3(0.3, 0, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(r.size.x, 0, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(r.size.x, 0, 0) + Vector3(0, 0.3, 0)); + + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(0, r.size.y, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(0, r.size.y, 0) - Vector3(0, 0.3, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(0, r.size.y, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + Vector3(0, r.size.y, 0) + Vector3(0.3, 0, 0)); + + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + r.size); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + r.size - Vector3(0.3, 0, 0)); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + r.size); + point_lines_mesh->surface_set_color(Color(0.8, 0.8, 0.8, 0.2)); + point_lines_mesh->surface_add_vertex(r.position + r.size - Vector3(0.0, 0.3, 0)); + + point_lines_mesh->surface_end(); + + if (poly.size() == 0) { + return; + } + + Array point_handle_mesh_array; + point_handle_mesh_array.resize(Mesh::ARRAY_MAX); + Vector point_handle_mesh_vertices; + + point_handle_mesh_vertices.resize(poly.size()); + Vector3 *point_handle_mesh_vertices_ptr = point_handle_mesh_vertices.ptrw(); + + for (int i = 0; i < poly.size(); i++) { + Vector2 point_2d; + Vector2 p2; + + if (i == edited_point) { + point_2d = edited_point_pos; + } else { + point_2d = poly[i]; + } + + Vector3 point_handle_3d = Vector3(point_2d.x, 0.0, point_2d.y); + point_handle_mesh_vertices_ptr[i] = point_handle_3d; + } + + point_handle_mesh_array[Mesh::ARRAY_VERTEX] = point_handle_mesh_vertices; + point_handle_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_POINTS, point_handle_mesh_array); + point_handle_mesh->surface_set_material(0, handle_material); +} + +void NavigationObstacle3DEditor::edit(Node *p_node) { + obstacle_node = Object::cast_to(p_node); + + if (obstacle_node) { + //Enable the pencil tool if the polygon is empty + if (_get_polygon().is_empty()) { + _menu_option(MODE_CREATE); + } + wip.clear(); + wip_active = false; + edited_point = -1; + p_node->add_child(point_lines_meshinstance); + _polygon_draw(); + + } else { + obstacle_node = nullptr; + + if (point_lines_meshinstance->get_parent()) { + point_lines_meshinstance->get_parent()->remove_child(point_lines_meshinstance); + } + } +} + +void NavigationObstacle3DEditor::_bind_methods() { + ClassDB::bind_method(D_METHOD("_polygon_draw"), &NavigationObstacle3DEditor::_polygon_draw); +} + +NavigationObstacle3DEditor::NavigationObstacle3DEditor() { + obstacle_node = nullptr; + + add_child(memnew(VSeparator)); + button_create = memnew(Button); + button_create->set_flat(true); + add_child(button_create); + button_create->connect("pressed", callable_mp(this, &NavigationObstacle3DEditor::_menu_option).bind(MODE_CREATE)); + button_create->set_toggle_mode(true); + + button_edit = memnew(Button); + button_edit->set_flat(true); + add_child(button_edit); + button_edit->connect("pressed", callable_mp(this, &NavigationObstacle3DEditor::_menu_option).bind(MODE_EDIT)); + button_edit->set_toggle_mode(true); + + mode = MODE_EDIT; + wip_active = false; + point_lines_meshinstance = memnew(MeshInstance3D); + point_lines_mesh.instantiate(); + point_lines_meshinstance->set_mesh(point_lines_mesh); + point_lines_meshinstance->set_transform(Transform3D(Basis(), Vector3(0, 0, 0.00001))); + + line_material = Ref(memnew(StandardMaterial3D)); + line_material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + line_material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + line_material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); + line_material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true); + line_material->set_albedo(Color(1, 1, 1)); + + handle_material = Ref(memnew(StandardMaterial3D)); + handle_material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + handle_material->set_flag(StandardMaterial3D::FLAG_USE_POINT_SIZE, true); + handle_material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + handle_material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true); + handle_material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true); + Ref handle = EditorNode::get_singleton()->get_gui_base()->get_theme_icon(SNAME("Editor3DHandle"), SNAME("EditorIcons")); + handle_material->set_point_size(handle->get_width()); + handle_material->set_texture(StandardMaterial3D::TEXTURE_ALBEDO, handle); + + point_handles_meshinstance = memnew(MeshInstance3D); + point_lines_meshinstance->add_child(point_handles_meshinstance); + point_handle_mesh.instantiate(); + point_handles_meshinstance->set_mesh(point_handle_mesh); + point_handles_meshinstance->set_transform(Transform3D(Basis(), Vector3(0, 0, 0.00001))); + + snap_ignore = false; +} + +NavigationObstacle3DEditor::~NavigationObstacle3DEditor() { + memdelete(point_lines_meshinstance); +} + +void NavigationObstacle3DEditorPlugin::edit(Object *p_object) { + obstacle_editor->edit(Object::cast_to(p_object)); +} + +bool NavigationObstacle3DEditorPlugin::handles(Object *p_object) const { + return Object::cast_to(p_object); +} + +void NavigationObstacle3DEditorPlugin::make_visible(bool p_visible) { + if (p_visible) { + obstacle_editor->show(); + } else { + obstacle_editor->hide(); + obstacle_editor->edit(nullptr); + } +} + +NavigationObstacle3DEditorPlugin::NavigationObstacle3DEditorPlugin() { + obstacle_editor = memnew(NavigationObstacle3DEditor); + Node3DEditor::get_singleton()->add_control_to_menu_panel(obstacle_editor); + + obstacle_editor->hide(); +} + +NavigationObstacle3DEditorPlugin::~NavigationObstacle3DEditorPlugin() { +} diff --git a/editor/plugins/navigation_obstacle_3d_editor_plugin.h b/editor/plugins/navigation_obstacle_3d_editor_plugin.h new file mode 100644 index 00000000000..1b125873d11 --- /dev/null +++ b/editor/plugins/navigation_obstacle_3d_editor_plugin.h @@ -0,0 +1,117 @@ +/**************************************************************************/ +/* navigation_obstacle_3d_editor_plugin.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef NAVIGATION_OBSTACLE_3D_EDITOR_PLUGIN_H +#define NAVIGATION_OBSTACLE_3D_EDITOR_PLUGIN_H + +#include "editor/editor_plugin.h" +#include "scene/3d/collision_polygon_3d.h" +#include "scene/3d/mesh_instance_3d.h" +#include "scene/gui/box_container.h" +#include "scene/resources/immediate_mesh.h" + +#include "scene/3d/navigation_obstacle_3d.h" + +class CanvasItemEditor; +class MenuButton; + +class NavigationObstacle3DEditor : public HBoxContainer { + GDCLASS(NavigationObstacle3DEditor, HBoxContainer); + + enum Mode { + MODE_CREATE, + MODE_EDIT, + + }; + + Mode mode; + + Button *button_create = nullptr; + Button *button_edit = nullptr; + + Ref line_material; + Ref handle_material; + + Panel *panel = nullptr; + NavigationObstacle3D *obstacle_node = nullptr; + Ref point_lines_mesh; + MeshInstance3D *point_lines_meshinstance = nullptr; + MeshInstance3D *point_handles_meshinstance = nullptr; + Ref point_handle_mesh; + + MenuButton *options = nullptr; + + int edited_point = 0; + Vector2 edited_point_pos; + PackedVector2Array pre_move_edit; + PackedVector2Array wip; + bool wip_active; + bool snap_ignore; + + float prev_depth = 0.0f; + + void _wip_close(); + void _polygon_draw(); + void _menu_option(int p_option); + + PackedVector2Array _get_polygon(); + void _set_polygon(PackedVector2Array p_poly); + +protected: + void _notification(int p_what); + void _node_removed(Node *p_node); + static void _bind_methods(); + +public: + virtual EditorPlugin::AfterGUIInput forward_3d_gui_input(Camera3D *p_camera, const Ref &p_event); + void edit(Node *p_node); + NavigationObstacle3DEditor(); + ~NavigationObstacle3DEditor(); +}; + +class NavigationObstacle3DEditorPlugin : public EditorPlugin { + GDCLASS(NavigationObstacle3DEditorPlugin, EditorPlugin); + + NavigationObstacle3DEditor *obstacle_editor = nullptr; + +public: + virtual EditorPlugin::AfterGUIInput forward_3d_gui_input(Camera3D *p_camera, const Ref &p_event) override { return obstacle_editor->forward_3d_gui_input(p_camera, p_event); } + + virtual String get_name() const override { return "NavigationObstacle3DEditor"; } + bool has_main_screen() const override { return false; } + virtual void edit(Object *p_object) override; + virtual bool handles(Object *p_object) const override; + virtual void make_visible(bool p_visible) override; + + NavigationObstacle3DEditorPlugin(); + ~NavigationObstacle3DEditorPlugin(); +}; + +#endif // NAVIGATION_OBSTACLE_3D_EDITOR_PLUGIN_H diff --git a/editor/register_editor_types.cpp b/editor/register_editor_types.cpp index 0a63c40bf61..1096003c402 100644 --- a/editor/register_editor_types.cpp +++ b/editor/register_editor_types.cpp @@ -81,6 +81,8 @@ #include "editor/plugins/mesh_library_editor_plugin.h" #include "editor/plugins/multimesh_editor_plugin.h" #include "editor/plugins/navigation_link_2d_editor_plugin.h" +#include "editor/plugins/navigation_obstacle_2d_editor_plugin.h" +#include "editor/plugins/navigation_obstacle_3d_editor_plugin.h" #include "editor/plugins/navigation_polygon_editor_plugin.h" #include "editor/plugins/node_3d_editor_gizmos.h" #include "editor/plugins/occluder_instance_3d_editor_plugin.h" @@ -185,6 +187,7 @@ void register_editor_types() { EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); + EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); @@ -213,6 +216,7 @@ void register_editor_types() { EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); + EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); EditorPlugins::add_by_type(); diff --git a/main/main.cpp b/main/main.cpp index b657fc61bb2..baa8692a87e 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -209,6 +209,7 @@ static bool use_debug_profiler = false; static bool debug_collisions = false; static bool debug_paths = false; static bool debug_navigation = false; +static bool debug_avoidance = false; #endif static int frame_delay = 0; static bool disable_render_loop = false; @@ -456,6 +457,7 @@ void Main::print_help(const char *p_binary) { OS::get_singleton()->print(" --debug-collisions Show collision shapes when running the scene.\n"); OS::get_singleton()->print(" --debug-paths Show path lines when running the scene.\n"); OS::get_singleton()->print(" --debug-navigation Show navigation polygons when running the scene.\n"); + OS::get_singleton()->print(" --debug-avoidance Show navigation polygons when running the scene.\n"); OS::get_singleton()->print(" --debug-stringnames Print all StringName allocations to stdout when the engine quits.\n"); #endif OS::get_singleton()->print(" --frame-delay Simulate high CPU load (delay each frame by milliseconds).\n"); @@ -1308,6 +1310,8 @@ Error Main::setup(const char *execpath, int argc, char *argv[], bool p_second_ph debug_paths = true; } else if (I->get() == "--debug-navigation") { debug_navigation = true; + } else if (I->get() == "--debug-avoidance") { + debug_avoidance = true; } else if (I->get() == "--debug-stringnames") { StringName::set_debug_stringnames(true); #endif @@ -2846,8 +2850,19 @@ bool Main::start() { } if (debug_navigation) { sml->set_debug_navigation_hint(true); + } + if (debug_avoidance) { + sml->set_debug_avoidance_hint(true); + } + if (debug_navigation || debug_avoidance) { NavigationServer3D::get_singleton()->set_active(true); NavigationServer3D::get_singleton()->set_debug_enabled(true); + if (debug_navigation) { + NavigationServer3D::get_singleton()->set_debug_navigation_enabled(true); + } + if (debug_avoidance) { + NavigationServer3D::get_singleton()->set_debug_avoidance_enabled(true); + } } #endif diff --git a/modules/gdscript/doc_classes/@GDScript.xml b/modules/gdscript/doc_classes/@GDScript.xml index e3129e848c8..e3f5502391d 100644 --- a/modules/gdscript/doc_classes/@GDScript.xml +++ b/modules/gdscript/doc_classes/@GDScript.xml @@ -458,6 +458,16 @@ [/codeblock] + + + + Export an integer property as a bit flag field for navigation avoidance layers. The widget in the Inspector dock will use the layer names defined in [member ProjectSettings.layer_names/avoidance/layer_1]. + See also [constant PROPERTY_HINT_LAYERS_AVOIDANCE]. + [codeblock] + @export_flags_avoidance var avoidance_layers: int + [/codeblock] + + diff --git a/modules/gdscript/gdscript_parser.cpp b/modules/gdscript/gdscript_parser.cpp index d3529154cfc..3bce2580727 100644 --- a/modules/gdscript/gdscript_parser.cpp +++ b/modules/gdscript/gdscript_parser.cpp @@ -104,6 +104,7 @@ GDScriptParser::GDScriptParser() { register_annotation(MethodInfo("@export_flags_3d_render"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations); register_annotation(MethodInfo("@export_flags_3d_physics"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations); register_annotation(MethodInfo("@export_flags_3d_navigation"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations); + register_annotation(MethodInfo("@export_flags_avoidance"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations); // Export grouping annotations. register_annotation(MethodInfo("@export_category", PropertyInfo(Variant::STRING, "name")), AnnotationInfo::STANDALONE, &GDScriptParser::export_group_annotations); register_annotation(MethodInfo("@export_group", PropertyInfo(Variant::STRING, "name"), PropertyInfo(Variant::STRING, "prefix")), AnnotationInfo::STANDALONE, &GDScriptParser::export_group_annotations, varray("")); diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp index db8c645558f..c77fa98be2d 100644 --- a/modules/gridmap/grid_map.cpp +++ b/modules/gridmap/grid_map.cpp @@ -907,7 +907,7 @@ void GridMap::_notification(int p_what) { #ifdef DEBUG_ENABLED case NOTIFICATION_ENTER_TREE: { - if (bake_navigation && NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (bake_navigation && NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { _update_navigation_debug_edge_connections(); } } break; @@ -1352,7 +1352,7 @@ void GridMap::_update_octant_navigation_debug_edge_connections_mesh(const Octant ERR_FAIL_COND(!octant_map.has(p_key)); Octant &g = *octant_map[p_key]; - if (!NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (!NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { if (g.navigation_debug_edge_connections_instance.is_valid()) { RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false); } diff --git a/modules/navigation/SCsub b/modules/navigation/SCsub index a9277657f4f..46bcb0fba4c 100644 --- a/modules/navigation/SCsub +++ b/modules/navigation/SCsub @@ -33,12 +33,30 @@ if env["builtin_recastnavigation"]: env_thirdparty.disable_warnings() env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) -# RVO Thirdparty source files -if env["builtin_rvo2"]: - thirdparty_dir = "#thirdparty/rvo2/" +# RVO 2D Thirdparty source files +if env["builtin_rvo2_2d"]: + thirdparty_dir = "#thirdparty/rvo2/rvo2_2d/" thirdparty_sources = [ - "Agent.cpp", - "KdTree.cpp", + "Agent2d.cpp", + "Obstacle2d.cpp", + "KdTree2d.cpp", + "RVOSimulator2d.cpp", + ] + thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] + + env_navigation.Prepend(CPPPATH=[thirdparty_dir]) + + env_thirdparty = env_navigation.Clone() + env_thirdparty.disable_warnings() + env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources) + +# RVO 3D Thirdparty source files +if env["builtin_rvo2_3d"]: + thirdparty_dir = "#thirdparty/rvo2/rvo2_3d/" + thirdparty_sources = [ + "Agent3d.cpp", + "KdTree3d.cpp", + "RVOSimulator3d.cpp", ] thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] @@ -51,7 +69,6 @@ if env["builtin_rvo2"]: env.modules_sources += thirdparty_obj - # Godot source files module_obj = [] diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp index ee9687cf3d2..63423e655c1 100644 --- a/modules/navigation/godot_navigation_server.cpp +++ b/modules/navigation/godot_navigation_server.cpp @@ -271,6 +271,18 @@ TypedArray GodotNavigationServer::map_get_agents(RID p_map) const { return agents_rids; } +TypedArray GodotNavigationServer::map_get_obstacles(RID p_map) const { + TypedArray obstacles_rids; + const NavMap *map = map_owner.get_or_null(p_map); + ERR_FAIL_COND_V(map == nullptr, obstacles_rids); + const LocalVector obstacles = map->get_obstacles(); + obstacles_rids.resize(obstacles.size()); + for (uint32_t i = 0; i < obstacles.size(); i++) { + obstacles_rids[i] = obstacles[i]->get_self(); + } + return obstacles_rids; +} + RID GodotNavigationServer::region_get_map(RID p_region) const { NavRegion *region = region_owner.get_or_null(p_region); ERR_FAIL_COND_V(region == nullptr, RID()); @@ -584,6 +596,34 @@ RID GodotNavigationServer::agent_create() { return rid; } +COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled) { + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->set_avoidance_enabled(p_enabled); +} + +bool GodotNavigationServer::agent_get_avoidance_enabled(RID p_agent) const { + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND_V(agent == nullptr, false); + + return agent->is_avoidance_enabled(); +} + +COMMAND_2(agent_set_use_3d_avoidance, RID, p_agent, bool, p_enabled) { + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->set_use_3d_avoidance(p_enabled); +} + +bool GodotNavigationServer::agent_get_use_3d_avoidance(RID p_agent) const { + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND_V(agent == nullptr, false); + + return agent->get_use_3d_avoidance(); +} + COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); @@ -605,7 +645,7 @@ COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) { agent->set_map(map); map->add_agent(agent); - if (agent->has_callback()) { + if (agent->has_avoidance_callback()) { map->set_agent_as_controlled(agent); } } @@ -615,63 +655,75 @@ COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->neighborDist_ = p_distance; + agent->set_neighbor_distance(p_distance); } COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->maxNeighbors_ = p_count; + agent->set_max_neighbors(p_count); } -COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time) { +COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon) { + ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive."); NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->timeHorizon_ = p_time; + agent->set_time_horizon_agents(p_time_horizon); +} + +COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon) { + ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive."); + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->set_time_horizon_obstacles(p_time_horizon); } COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) { + ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive."); NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->radius_ = p_radius; + agent->set_radius(p_radius); +} + +COMMAND_2(agent_set_height, RID, p_agent, real_t, p_height) { + ERR_FAIL_COND_MSG(p_height < 0.0, "Height must be positive."); + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->set_height(p_height); } COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) { + ERR_FAIL_COND_MSG(p_max_speed < 0.0, "Max speed must be positive."); NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->maxSpeed_ = p_max_speed; + agent->set_max_speed(p_max_speed); } COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->velocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z); + agent->set_velocity(p_velocity); } -COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity) { +COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector3, p_velocity) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->get_agent()->prefVelocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z); + agent->set_velocity_forced(p_velocity); } COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - 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) { - NavAgent *agent = agent_owner.get_or_null(p_agent); - ERR_FAIL_COND(agent == nullptr); - - agent->get_agent()->ignore_y_ = p_ignore; + agent->set_position(p_position); } bool GodotNavigationServer::agent_is_map_changed(RID p_agent) const { @@ -681,11 +733,11 @@ bool GodotNavigationServer::agent_is_map_changed(RID p_agent) const { return agent->is_map_changed(); } -COMMAND_2(agent_set_callback, RID, p_agent, Callable, p_callback) { +COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback) { NavAgent *agent = agent_owner.get_or_null(p_agent); ERR_FAIL_COND(agent == nullptr); - agent->set_callback(p_callback); + agent->set_avoidance_callback(p_callback); if (agent->get_map()) { if (p_callback.is_valid()) { @@ -696,6 +748,91 @@ COMMAND_2(agent_set_callback, RID, p_agent, Callable, p_callback) { } } +COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers) { + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + agent->set_avoidance_layers(p_layers); +} + +COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask) { + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + agent->set_avoidance_mask(p_mask); +} + +COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority) { + ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + NavAgent *agent = agent_owner.get_or_null(p_agent); + ERR_FAIL_COND(agent == nullptr); + agent->set_avoidance_priority(p_priority); +} + +RID GodotNavigationServer::obstacle_create() { + GodotNavigationServer *mut_this = const_cast(this); + MutexLock lock(mut_this->operations_mutex); + RID rid = obstacle_owner.make_rid(); + NavObstacle *obstacle = obstacle_owner.get_or_null(rid); + obstacle->set_self(rid); + return rid; +} + +COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map) { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle); + ERR_FAIL_COND(obstacle == nullptr); + + if (obstacle->get_map()) { + if (obstacle->get_map()->get_self() == p_map) { + return; // Pointless + } + + obstacle->get_map()->remove_obstacle(obstacle); + } + + obstacle->set_map(nullptr); + + if (p_map.is_valid()) { + NavMap *map = map_owner.get_or_null(p_map); + ERR_FAIL_COND(map == nullptr); + + obstacle->set_map(map); + map->add_obstacle(obstacle); + } +} + +RID GodotNavigationServer::obstacle_get_map(RID p_obstacle) const { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle); + ERR_FAIL_COND_V(obstacle == nullptr, RID()); + if (obstacle->get_map()) { + return obstacle->get_map()->get_self(); + } + return RID(); +} + +COMMAND_2(obstacle_set_height, RID, p_obstacle, real_t, p_height) { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle); + ERR_FAIL_COND(obstacle == nullptr); + obstacle->set_height(p_height); +} + +COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position) { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle); + ERR_FAIL_COND(obstacle == nullptr); + obstacle->set_position(p_position); +} + +void GodotNavigationServer::obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle); + ERR_FAIL_COND(obstacle == nullptr); + obstacle->set_vertices(p_vertices); +} + +COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers) { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle); + ERR_FAIL_COND(obstacle == nullptr); + obstacle->set_avoidance_layers(p_layers); +} + COMMAND_1(free, RID, p_object) { if (map_owner.owns(p_object)) { NavMap *map = map_owner.get_or_null(p_object); @@ -718,6 +855,12 @@ COMMAND_1(free, RID, p_object) { agent->set_map(nullptr); } + // Remove any assigned obstacles + for (NavObstacle *obstacle : map->get_obstacles()) { + map->remove_obstacle(obstacle); + obstacle->set_map(nullptr); + } + int map_index = active_maps.find(map); active_maps.remove_at(map_index); active_maps_update_id.remove_at(map_index); @@ -756,6 +899,17 @@ COMMAND_1(free, RID, p_object) { agent_owner.free(p_object); + } else if (obstacle_owner.owns(p_object)) { + NavObstacle *obstacle = obstacle_owner.get_or_null(p_object); + + // Removes this agent from the map if assigned + if (obstacle->get_map() != nullptr) { + obstacle->get_map()->remove_obstacle(obstacle); + obstacle->set_map(nullptr); + } + + obstacle_owner.free(p_object); + } else { ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed)."); } diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h index 0b113b77d4e..ee9b1f05b74 100644 --- a/modules/navigation/godot_navigation_server.h +++ b/modules/navigation/godot_navigation_server.h @@ -39,6 +39,7 @@ #include "nav_agent.h" #include "nav_link.h" #include "nav_map.h" +#include "nav_obstacle.h" #include "nav_region.h" /// The commands are functions executed during the `sync` phase. @@ -72,6 +73,7 @@ class GodotNavigationServer : public NavigationServer3D { mutable RID_Owner map_owner; mutable RID_Owner region_owner; mutable RID_Owner agent_owner; + mutable RID_Owner obstacle_owner; bool active = true; LocalVector active_maps; @@ -121,6 +123,7 @@ public: virtual TypedArray map_get_links(RID p_map) const override; virtual TypedArray map_get_regions(RID p_map) const override; virtual TypedArray map_get_agents(RID p_map) const override; + virtual TypedArray map_get_obstacles(RID p_map) const override; virtual void map_force_update(RID p_map) override; @@ -166,19 +169,35 @@ public: virtual ObjectID link_get_owner_id(RID p_link) const override; virtual RID agent_create() override; + COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled); + virtual bool agent_get_avoidance_enabled(RID p_agent) const override; + COMMAND_2(agent_set_use_3d_avoidance, RID, p_agent, bool, p_enabled); + virtual bool agent_get_use_3d_avoidance(RID p_agent) const override; COMMAND_2(agent_set_map, RID, p_agent, RID, p_map); virtual RID agent_get_map(RID p_agent) const override; COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance); 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_time_horizon_agents, RID, p_agent, real_t, p_time_horizon); + COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon); COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius); + COMMAND_2(agent_set_height, RID, p_agent, real_t, p_height); 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_velocity_forced, 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 override; - COMMAND_2(agent_set_callback, RID, p_agent, Callable, p_callback); + COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback); + COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers); + COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask); + COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority); + + virtual RID obstacle_create() override; + COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map); + virtual RID obstacle_get_map(RID p_obstacle) const override; + COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position); + COMMAND_2(obstacle_set_height, RID, p_obstacle, real_t, p_height); + virtual void obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) override; + COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers); COMMAND_1(free, RID, p_object); diff --git a/modules/navigation/nav_agent.cpp b/modules/navigation/nav_agent.cpp index 293544c0a53..a0efe4c74c6 100644 --- a/modules/navigation/nav_agent.cpp +++ b/modules/navigation/nav_agent.cpp @@ -32,8 +32,66 @@ #include "nav_map.h" +NavAgent::NavAgent() { +} + +void NavAgent::set_avoidance_enabled(bool p_enabled) { + avoidance_enabled = p_enabled; + _update_rvo_agent_properties(); +} + +void NavAgent::set_use_3d_avoidance(bool p_enabled) { + use_3d_avoidance = p_enabled; + _update_rvo_agent_properties(); +} + +void NavAgent::_update_rvo_agent_properties() { + if (use_3d_avoidance) { + rvo_agent_3d.neighborDist_ = neighbor_distance; + rvo_agent_3d.maxNeighbors_ = max_neighbors; + rvo_agent_3d.timeHorizon_ = time_horizon_agents; + rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles; + rvo_agent_3d.radius_ = radius; + rvo_agent_3d.maxSpeed_ = max_speed; + rvo_agent_3d.position_ = RVO3D::Vector3(position.x, position.y, position.z); + // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing. + //rvo_agent_3d.velocity_ = RVO3D::Vector3(velocity.x, velocity.y ,velocity.z); + rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z); + rvo_agent_3d.height_ = height; + rvo_agent_3d.avoidance_layers_ = avoidance_layers; + rvo_agent_3d.avoidance_mask_ = avoidance_mask; + rvo_agent_3d.avoidance_priority_ = avoidance_priority; + } else { + rvo_agent_2d.neighborDist_ = neighbor_distance; + rvo_agent_2d.maxNeighbors_ = max_neighbors; + rvo_agent_2d.timeHorizon_ = time_horizon_agents; + rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles; + rvo_agent_2d.radius_ = radius; + rvo_agent_2d.maxSpeed_ = max_speed; + rvo_agent_2d.position_ = RVO2D::Vector2(position.x, position.z); + rvo_agent_2d.elevation_ = position.y; + // Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing. + //rvo_agent_2d.velocity_ = RVO2D::Vector2(velocity.x, velocity.z); + rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z); + rvo_agent_2d.height_ = height; + rvo_agent_2d.avoidance_layers_ = avoidance_layers; + rvo_agent_2d.avoidance_mask_ = avoidance_mask; + rvo_agent_2d.avoidance_priority_ = avoidance_priority; + } + + if (map != nullptr) { + if (avoidance_enabled) { + map->set_agent_as_controlled(this); + } else { + map->remove_agent_as_controlled(this); + } + } + agent_dirty = true; +} + void NavAgent::set_map(NavMap *p_map) { map = p_map; + agent_dirty = true; } bool NavAgent::is_map_changed() { @@ -46,25 +104,241 @@ bool NavAgent::is_map_changed() { } } -void NavAgent::set_callback(Callable p_callback) { - callback = p_callback; +void NavAgent::set_avoidance_callback(Callable p_callback) { + avoidance_callback = p_callback; } -bool NavAgent::has_callback() const { - return callback.is_valid(); +bool NavAgent::has_avoidance_callback() const { + return avoidance_callback.is_valid(); } -void NavAgent::dispatch_callback() { - if (!callback.is_valid()) { +void NavAgent::dispatch_avoidance_callback() { + if (!avoidance_callback.is_valid()) { return; } - Vector3 new_velocity = Vector3(agent.newVelocity_.x(), agent.newVelocity_.y(), agent.newVelocity_.z()); + Vector3 new_velocity; + + if (use_3d_avoidance) { + new_velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z()); + } else { + new_velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y()); + } + + if (clamp_speed) { + new_velocity = new_velocity.limit_length(max_speed); + } // Invoke the callback with the new velocity. Variant args[] = { new_velocity }; const Variant *args_p[] = { &args[0] }; Variant return_value; Callable::CallError call_error; - callback.callp(args_p, 1, return_value, call_error); + + avoidance_callback.callp(args_p, 1, return_value, call_error); +} + +void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) { + neighbor_distance = p_neighbor_distance; + if (use_3d_avoidance) { + rvo_agent_3d.neighborDist_ = neighbor_distance; + } else { + rvo_agent_2d.neighborDist_ = neighbor_distance; + } + agent_dirty = true; +} + +void NavAgent::set_max_neighbors(int p_max_neighbors) { + max_neighbors = p_max_neighbors; + if (use_3d_avoidance) { + rvo_agent_3d.maxNeighbors_ = max_neighbors; + } else { + rvo_agent_2d.maxNeighbors_ = max_neighbors; + } + agent_dirty = true; +} + +void NavAgent::set_time_horizon_agents(real_t p_time_horizon) { + time_horizon_agents = p_time_horizon; + if (use_3d_avoidance) { + rvo_agent_3d.timeHorizon_ = time_horizon_agents; + } else { + rvo_agent_2d.timeHorizon_ = time_horizon_agents; + } + agent_dirty = true; +} + +void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) { + time_horizon_obstacles = p_time_horizon; + if (use_3d_avoidance) { + rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles; + } else { + rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles; + } + agent_dirty = true; +} + +void NavAgent::set_radius(real_t p_radius) { + radius = p_radius; + if (use_3d_avoidance) { + rvo_agent_3d.radius_ = radius; + } else { + rvo_agent_2d.radius_ = radius; + } + agent_dirty = true; +} + +void NavAgent::set_height(real_t p_height) { + height = p_height; + if (use_3d_avoidance) { + rvo_agent_3d.height_ = height; + } else { + rvo_agent_2d.height_ = height; + } + agent_dirty = true; +} + +void NavAgent::set_max_speed(real_t p_max_speed) { + max_speed = p_max_speed; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.maxSpeed_ = max_speed; + } else { + rvo_agent_2d.maxSpeed_ = max_speed; + } + } + agent_dirty = true; +} + +void NavAgent::set_position(const Vector3 p_position) { + position = p_position; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.position_ = RVO3D::Vector3(p_position.x, p_position.y, p_position.z); + } else { + rvo_agent_2d.elevation_ = p_position.y; + rvo_agent_2d.position_ = RVO2D::Vector2(p_position.x, p_position.z); + } + } + agent_dirty = true; +} + +void NavAgent::set_target_position(const Vector3 p_target_position) { + target_position = p_target_position; +} + +void NavAgent::set_velocity(const Vector3 p_velocity) { + // Sets the "wanted" velocity for an agent as a suggestion + // This velocity is not guaranteed, RVO simulation will only try to fulfill it + velocity = p_velocity; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z); + } else { + rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z); + } + } + agent_dirty = true; +} + +void NavAgent::set_velocity_forced(const Vector3 p_velocity) { + // This function replaces the internal rvo simulation velocity + // should only be used after the agent was teleported + // as it destroys consistency in movement in cramped situations + // use velocity instead to update with a safer "suggestion" + velocity_forced = p_velocity; + if (avoidance_enabled) { + if (use_3d_avoidance) { + rvo_agent_3d.velocity_ = RVO3D::Vector3(p_velocity.x, p_velocity.y, p_velocity.z); + } else { + rvo_agent_2d.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.z); + } + } + agent_dirty = true; +} + +void NavAgent::update() { + // Updates this agent with the calculated results from the rvo simulation + if (avoidance_enabled) { + if (use_3d_avoidance) { + velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z()); + } else { + velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y()); + } + } +} + +void NavAgent::set_avoidance_mask(uint32_t p_mask) { + avoidance_mask = p_mask; + if (use_3d_avoidance) { + rvo_agent_3d.avoidance_mask_ = avoidance_mask; + } else { + rvo_agent_2d.avoidance_mask_ = avoidance_mask; + } + agent_dirty = true; +} + +void NavAgent::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + if (use_3d_avoidance) { + rvo_agent_3d.avoidance_layers_ = avoidance_layers; + } else { + rvo_agent_2d.avoidance_layers_ = avoidance_layers; + } + agent_dirty = true; +} + +void NavAgent::set_avoidance_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + avoidance_priority = p_priority; + if (use_3d_avoidance) { + rvo_agent_3d.avoidance_priority_ = avoidance_priority; + } else { + rvo_agent_2d.avoidance_priority_ = avoidance_priority; + } + agent_dirty = true; +}; + +bool NavAgent::check_dirty() { + const bool was_dirty = agent_dirty; + agent_dirty = false; + return was_dirty; +} + +const Dictionary NavAgent::get_avoidance_data() const { + // Returns debug data from RVO simulation internals of this agent. + Dictionary _avoidance_data; + if (use_3d_avoidance) { + _avoidance_data["max_neighbors"] = int(rvo_agent_3d.maxNeighbors_); + _avoidance_data["max_speed"] = float(rvo_agent_3d.maxSpeed_); + _avoidance_data["neighbor_distance"] = float(rvo_agent_3d.neighborDist_); + _avoidance_data["new_velocity"] = Vector3(rvo_agent_3d.newVelocity_.x(), rvo_agent_3d.newVelocity_.y(), rvo_agent_3d.newVelocity_.z()); + _avoidance_data["velocity"] = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z()); + _avoidance_data["position"] = Vector3(rvo_agent_3d.position_.x(), rvo_agent_3d.position_.y(), rvo_agent_3d.position_.z()); + _avoidance_data["prefered_velocity"] = Vector3(rvo_agent_3d.prefVelocity_.x(), rvo_agent_3d.prefVelocity_.y(), rvo_agent_3d.prefVelocity_.z()); + _avoidance_data["radius"] = float(rvo_agent_3d.radius_); + _avoidance_data["time_horizon_agents"] = float(rvo_agent_3d.timeHorizon_); + _avoidance_data["time_horizon_obstacles"] = 0.0; + _avoidance_data["height"] = float(rvo_agent_3d.height_); + _avoidance_data["avoidance_layers"] = int(rvo_agent_3d.avoidance_layers_); + _avoidance_data["avoidance_mask"] = int(rvo_agent_3d.avoidance_mask_); + _avoidance_data["avoidance_priority"] = float(rvo_agent_3d.avoidance_priority_); + } else { + _avoidance_data["max_neighbors"] = int(rvo_agent_2d.maxNeighbors_); + _avoidance_data["max_speed"] = float(rvo_agent_2d.maxSpeed_); + _avoidance_data["neighbor_distance"] = float(rvo_agent_2d.neighborDist_); + _avoidance_data["new_velocity"] = Vector3(rvo_agent_2d.newVelocity_.x(), 0.0, rvo_agent_2d.newVelocity_.y()); + _avoidance_data["velocity"] = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y()); + _avoidance_data["position"] = Vector3(rvo_agent_2d.position_.x(), 0.0, rvo_agent_2d.position_.y()); + _avoidance_data["prefered_velocity"] = Vector3(rvo_agent_2d.prefVelocity_.x(), 0.0, rvo_agent_2d.prefVelocity_.y()); + _avoidance_data["radius"] = float(rvo_agent_2d.radius_); + _avoidance_data["time_horizon_agents"] = float(rvo_agent_2d.timeHorizon_); + _avoidance_data["time_horizon_obstacles"] = float(rvo_agent_2d.timeHorizonObst_); + _avoidance_data["height"] = float(rvo_agent_2d.height_); + _avoidance_data["avoidance_layers"] = int(rvo_agent_2d.avoidance_layers_); + _avoidance_data["avoidance_mask"] = int(rvo_agent_2d.avoidance_mask_); + _avoidance_data["avoidance_priority"] = float(rvo_agent_2d.avoidance_priority_); + } + return _avoidance_data; } diff --git a/modules/navigation/nav_agent.h b/modules/navigation/nav_agent.h index f154ce14d92..497b239f84f 100644 --- a/modules/navigation/nav_agent.h +++ b/modules/navigation/nav_agent.h @@ -32,34 +32,121 @@ #define NAV_AGENT_H #include "core/object/class_db.h" +#include "core/templates/local_vector.h" +#include "nav_agent.h" #include "nav_rid.h" -#include +#include +#include class NavMap; class NavAgent : public NavRid { + Vector3 position; + Vector3 target_position; + Vector3 velocity; + Vector3 velocity_forced; + real_t height = 1.0; + real_t radius = 1.0; + real_t max_speed = 1.0; + real_t time_horizon_agents = 1.0; + real_t time_horizon_obstacles = 0.0; + int max_neighbors = 5; + real_t neighbor_distance = 5.0; + Vector3 safe_velocity; + bool clamp_speed = true; // Experimental, clamps velocity to max_speed. + NavMap *map = nullptr; - RVO::Agent agent; - Callable callback = Callable(); + + RVO2D::Agent2D rvo_agent_2d; + RVO3D::Agent3D rvo_agent_3d; + bool use_3d_avoidance = false; + bool avoidance_enabled = false; + + uint32_t avoidance_layers = 1; + uint32_t avoidance_mask = 1; + real_t avoidance_priority = 1.0; + + Callable avoidance_callback = Callable(); + + bool agent_dirty = true; + uint32_t map_update_id = 0; public: - void set_map(NavMap *p_map); - NavMap *get_map() { - return map; - } + NavAgent(); - RVO::Agent *get_agent() { - return &agent; - } + void set_avoidance_enabled(bool p_enabled); + bool is_avoidance_enabled() { return avoidance_enabled; } + + void set_use_3d_avoidance(bool p_enabled); + bool get_use_3d_avoidance() { return use_3d_avoidance; } + + void set_map(NavMap *p_map); + NavMap *get_map() { return map; } bool is_map_changed(); - void set_callback(Callable p_callback); - bool has_callback() const; + RVO2D::Agent2D *get_rvo_agent_2d() { return &rvo_agent_2d; } + RVO3D::Agent3D *get_rvo_agent_3d() { return &rvo_agent_3d; } - void dispatch_callback(); + void set_avoidance_callback(Callable p_callback); + bool has_avoidance_callback() const; + + void dispatch_avoidance_callback(); + + void set_neighbor_distance(real_t p_neighbor_distance); + real_t get_neighbor_distance() const { return neighbor_distance; } + + void set_max_neighbors(int p_max_neighbors); + int get_max_neighbors() const { return max_neighbors; } + + void set_time_horizon_agents(real_t p_time_horizon); + real_t get_time_horizon_agents() const { return time_horizon_agents; } + + void set_time_horizon_obstacles(real_t p_time_horizon); + real_t get_time_horizon_obstacles() const { return time_horizon_obstacles; } + + void set_radius(real_t p_radius); + real_t get_radius() const { return radius; } + + void set_height(real_t p_height); + real_t get_height() const { return height; } + + void set_max_speed(real_t p_max_speed); + real_t get_max_speed() const { return max_speed; } + + void set_position(const Vector3 p_position); + const Vector3 &get_position() const { return position; } + + void set_target_position(const Vector3 p_target_position); + const Vector3 &get_target_position() const { return target_position; } + + void set_velocity(const Vector3 p_velocity); + const Vector3 &get_velocity() const { return velocity; } + + void set_velocity_forced(const Vector3 p_velocity); + const Vector3 &get_velocity_forced() const { return velocity_forced; } + + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const { return avoidance_layers; }; + + void set_avoidance_mask(uint32_t p_mask); + uint32_t get_avoidance_mask() const { return avoidance_mask; }; + + void set_avoidance_priority(real_t p_priority); + real_t get_avoidance_priority() const { return avoidance_priority; }; + + bool check_dirty(); + + // Updates this agent with rvo data after the rvo simulation avoidance step. + void update(); + + // RVO debug data from the last frame update. + const Dictionary get_avoidance_data() const; + +private: + void _update_rvo_agent_properties(); }; #endif // NAV_AGENT_H diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 91b13ba9c4e..b76295db86d 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -30,11 +30,14 @@ #include "nav_map.h" +#include "core/config/project_settings.h" #include "core/object/worker_thread_pool.h" #include "nav_agent.h" #include "nav_link.h" +#include "nav_obstacle.h" #include "nav_region.h" -#include + +#include #define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) @@ -522,9 +525,7 @@ gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_poin gd::ClosestPointQueryResult result; real_t closest_point_ds = FLT_MAX; - for (size_t i(0); i < polygons.size(); i++) { - const gd::Polygon &p = polygons[i]; - + for (const gd::Polygon &p : polygons) { // For each face check the distance to the point for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); @@ -549,7 +550,7 @@ void NavMap::add_region(NavRegion *p_region) { void NavMap::remove_region(NavRegion *p_region) { int64_t region_index = regions.find(p_region); - if (region_index != -1) { + if (region_index >= 0) { regions.remove_at_unordered(region_index); regenerate_links = true; } @@ -562,14 +563,14 @@ void NavMap::add_link(NavLink *p_link) { void NavMap::remove_link(NavLink *p_link) { int64_t link_index = links.find(p_link); - if (link_index != -1) { + if (link_index >= 0) { links.remove_at_unordered(link_index); regenerate_links = true; } } bool NavMap::has_agent(NavAgent *agent) const { - return (agents.find(agent) != -1); + return (agents.find(agent) >= 0); } void NavMap::add_agent(NavAgent *agent) { @@ -582,25 +583,57 @@ void NavMap::add_agent(NavAgent *agent) { void NavMap::remove_agent(NavAgent *agent) { remove_agent_as_controlled(agent); int64_t agent_index = agents.find(agent); - if (agent_index != -1) { + if (agent_index >= 0) { agents.remove_at_unordered(agent_index); agents_dirty = true; } } +bool NavMap::has_obstacle(NavObstacle *obstacle) const { + return (obstacles.find(obstacle) >= 0); +} + +void NavMap::add_obstacle(NavObstacle *obstacle) { + if (!has_obstacle(obstacle)) { + obstacles.push_back(obstacle); + obstacles_dirty = true; + } +} + +void NavMap::remove_obstacle(NavObstacle *obstacle) { + int64_t obstacle_index = obstacles.find(obstacle); + if (obstacle_index >= 0) { + obstacles.remove_at_unordered(obstacle_index); + obstacles_dirty = true; + } +} + void NavMap::set_agent_as_controlled(NavAgent *agent) { - const bool exist = (controlled_agents.find(agent) != -1); - if (!exist) { - ERR_FAIL_COND(!has_agent(agent)); - controlled_agents.push_back(agent); - agents_dirty = true; + remove_agent_as_controlled(agent); + if (agent->get_use_3d_avoidance()) { + int64_t agent_3d_index = active_3d_avoidance_agents.find(agent); + if (agent_3d_index < 0) { + active_3d_avoidance_agents.push_back(agent); + agents_dirty = true; + } + } else { + int64_t agent_2d_index = active_2d_avoidance_agents.find(agent); + if (agent_2d_index < 0) { + active_2d_avoidance_agents.push_back(agent); + agents_dirty = true; + } } } void NavMap::remove_agent_as_controlled(NavAgent *agent) { - int64_t active_avoidance_agent_index = controlled_agents.find(agent); - if (active_avoidance_agent_index != -1) { - controlled_agents.remove_at_unordered(active_avoidance_agent_index); + int64_t agent_3d_index = active_3d_avoidance_agents.find(agent); + if (agent_3d_index >= 0) { + active_3d_avoidance_agents.remove_at_unordered(agent_3d_index); + agents_dirty = true; + } + int64_t agent_2d_index = active_2d_avoidance_agents.find(agent); + if (agent_2d_index >= 0) { + active_2d_avoidance_agents.remove_at_unordered(agent_2d_index); agents_dirty = true; } } @@ -891,22 +924,30 @@ void NavMap::sync() { map_update_id = (map_update_id + 1) % 9999999; } - // Update agents tree. - if (agents_dirty) { - // cannot use LocalVector here as RVO library expects std::vector to build KdTree - std::vector raw_agents; - raw_agents.reserve(controlled_agents.size()); - for (NavAgent *controlled_agent : controlled_agents) { - raw_agents.push_back(controlled_agent->get_agent()); + // Do we have modified obstacle positions? + for (NavObstacle *obstacle : obstacles) { + if (obstacle->check_dirty()) { + obstacles_dirty = true; } - rvo.buildAgentTree(raw_agents); + } + // Do we have modified agent arrays? + for (NavAgent *agent : agents) { + if (agent->check_dirty()) { + agents_dirty = true; + } + } + + // Update avoidance worlds. + if (obstacles_dirty || agents_dirty) { + _update_rvo_simulation(); } regenerate_polygons = false; regenerate_links = false; + obstacles_dirty = false; agents_dirty = false; - // Performance Monitor + // Performance Monitor. pm_region_count = _new_pm_region_count; pm_agent_count = _new_pm_agent_count; pm_link_count = _new_pm_link_count; @@ -917,22 +958,155 @@ void NavMap::sync() { pm_edge_free_count = _new_pm_edge_free_count; } -void NavMap::compute_single_step(uint32_t index, NavAgent **agent) { - (*(agent + index))->get_agent()->computeNeighbors(&rvo); - (*(agent + index))->get_agent()->computeNewVelocity(deltatime); +void NavMap::_update_rvo_obstacles_tree_2d() { + int obstacle_vertex_count = 0; + for (NavObstacle *obstacle : obstacles) { + obstacle_vertex_count += obstacle->get_vertices().size(); + } + + // Cannot use LocalVector here as RVO library expects std::vector to build KdTree + std::vector raw_obstacles; + raw_obstacles.reserve(obstacle_vertex_count); + + // The following block is modified copy from RVO2D::AddObstacle() + // Obstacles are linked and depend on all other obstacles. + for (NavObstacle *obstacle : obstacles) { + const Vector3 &_obstacle_position = obstacle->get_position(); + const Vector &_obstacle_vertices = obstacle->get_vertices(); + + if (_obstacle_vertices.size() < 2) { + continue; + } + + std::vector rvo_2d_vertices; + rvo_2d_vertices.reserve(_obstacle_vertices.size()); + + uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers(); + + for (const Vector3 &_obstacle_vertex : _obstacle_vertices) { + rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z)); + } + + const size_t obstacleNo = raw_obstacles.size(); + + for (size_t i = 0; i < rvo_2d_vertices.size(); i++) { + RVO2D::Obstacle2D *rvo_2d_obstacle = new RVO2D::Obstacle2D(); + rvo_2d_obstacle->point_ = rvo_2d_vertices[i]; + rvo_2d_obstacle->avoidance_layers_ = _obstacle_avoidance_layers; + + if (i != 0) { + rvo_2d_obstacle->prevObstacle_ = raw_obstacles.back(); + rvo_2d_obstacle->prevObstacle_->nextObstacle_ = rvo_2d_obstacle; + } + + if (i == rvo_2d_vertices.size() - 1) { + rvo_2d_obstacle->nextObstacle_ = raw_obstacles[obstacleNo]; + rvo_2d_obstacle->nextObstacle_->prevObstacle_ = rvo_2d_obstacle; + } + + rvo_2d_obstacle->unitDir_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]); + + if (rvo_2d_vertices.size() == 2) { + rvo_2d_obstacle->isConvex_ = true; + } else { + rvo_2d_obstacle->isConvex_ = (leftOf(rvo_2d_vertices[(i == 0 ? rvo_2d_vertices.size() - 1 : i - 1)], rvo_2d_vertices[i], rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); + } + + rvo_2d_obstacle->id_ = raw_obstacles.size(); + + raw_obstacles.push_back(rvo_2d_obstacle); + } + } + + rvo_simulation_2d.kdTree_->buildObstacleTree(raw_obstacles); +} + +void NavMap::_update_rvo_agents_tree_2d() { + // Cannot use LocalVector here as RVO library expects std::vector to build KdTree. + std::vector raw_agents; + raw_agents.reserve(active_2d_avoidance_agents.size()); + for (NavAgent *agent : active_2d_avoidance_agents) { + raw_agents.push_back(agent->get_rvo_agent_2d()); + } + rvo_simulation_2d.kdTree_->buildAgentTree(raw_agents); +} + +void NavMap::_update_rvo_agents_tree_3d() { + // Cannot use LocalVector here as RVO library expects std::vector to build KdTree. + std::vector raw_agents; + raw_agents.reserve(active_3d_avoidance_agents.size()); + for (NavAgent *agent : active_3d_avoidance_agents) { + raw_agents.push_back(agent->get_rvo_agent_3d()); + } + rvo_simulation_3d.kdTree_->buildAgentTree(raw_agents); +} + +void NavMap::_update_rvo_simulation() { + if (obstacles_dirty) { + _update_rvo_obstacles_tree_2d(); + } + if (agents_dirty) { + _update_rvo_agents_tree_2d(); + _update_rvo_agents_tree_3d(); + } +} + +void NavMap::compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent) { + (*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d); + (*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d); + (*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d); + (*(agent + index))->update(); +} + +void NavMap::compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent) { + (*(agent + index))->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d); + (*(agent + index))->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d); + (*(agent + index))->get_rvo_agent_3d()->update(&rvo_simulation_3d); + (*(agent + index))->update(); } void NavMap::step(real_t p_deltatime) { deltatime = p_deltatime; - if (controlled_agents.size() > 0) { - WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_step, controlled_agents.ptr(), controlled_agents.size(), -1, true, SNAME("NavigationMapAgents")); - WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + + rvo_simulation_2d.setTimeStep(float(deltatime)); + rvo_simulation_3d.setTimeStep(float(deltatime)); + + if (active_2d_avoidance_agents.size() > 0) { + if (use_threads && avoidance_use_multiple_threads) { + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + } else { + for (NavAgent *agent : active_2d_avoidance_agents) { + agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d); + agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d); + agent->get_rvo_agent_2d()->update(&rvo_simulation_2d); + agent->update(); + } + } + } + + if (active_3d_avoidance_agents.size() > 0) { + if (use_threads && avoidance_use_multiple_threads) { + WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D")); + WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task); + } else { + for (NavAgent *agent : active_3d_avoidance_agents) { + agent->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d); + agent->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d); + agent->get_rvo_agent_3d()->update(&rvo_simulation_3d); + agent->update(); + } + } } } void NavMap::dispatch_callbacks() { - for (NavAgent *agent : controlled_agents) { - agent->dispatch_callback(); + for (NavAgent *agent : active_2d_avoidance_agents) { + agent->dispatch_avoidance_callback(); + } + + for (NavAgent *agent : active_3d_avoidance_agents) { + agent->dispatch_avoidance_callback(); } } @@ -970,6 +1144,8 @@ void NavMap::clip_path(const LocalVector &p_navigation_polys } NavMap::NavMap() { + avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads"); + avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads"); } NavMap::~NavMap() { diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h index 5ec2c2826c5..343f53760b9 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -38,11 +38,16 @@ #include "core/templates/rb_map.h" #include "nav_utils.h" -#include +#include +#include + +#include +#include class NavLink; class NavRegion; class NavAgent; +class NavObstacle; class NavMap : public NavRid { /// Map Up @@ -71,17 +76,25 @@ class NavMap : public NavRid { /// Map polygons LocalVector polygons; - /// Rvo world - RVO::KdTree rvo; + /// RVO avoidance worlds + RVO2D::RVOSimulator2D rvo_simulation_2d; + RVO3D::RVOSimulator3D rvo_simulation_3d; - /// Is agent array modified? - bool agents_dirty = false; + /// avoidance controlled agents + LocalVector active_2d_avoidance_agents; + LocalVector active_3d_avoidance_agents; + + /// dirty flag when one of the agent's arrays are modified + bool agents_dirty = true; /// All the Agents (even the controlled one) LocalVector agents; - /// Controlled agents - LocalVector controlled_agents; + /// All the avoidance obstacles (both static and dynamic) + LocalVector obstacles; + + /// Are rvo obstacles modified? + bool obstacles_dirty = true; /// Physics delta time real_t deltatime = 0.0; @@ -89,6 +102,10 @@ class NavMap : public NavRid { /// Change the id each time the map is updated. uint32_t map_update_id = 0; + bool use_threads = true; + bool avoidance_use_multiple_threads = true; + bool avoidance_use_high_priority_threads = true; + // Performance Monitor int pm_region_count = 0; int pm_agent_count = 0; @@ -154,6 +171,13 @@ public: void set_agent_as_controlled(NavAgent *agent); void remove_agent_as_controlled(NavAgent *agent); + bool has_obstacle(NavObstacle *obstacle) const; + void add_obstacle(NavObstacle *obstacle); + void remove_obstacle(NavObstacle *obstacle); + const LocalVector &get_obstacles() const { + return obstacles; + } + uint32_t get_map_update_id() const { return map_update_id; } @@ -174,7 +198,15 @@ public: private: void compute_single_step(uint32_t index, NavAgent **agent); + + void compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent); + void compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent); + void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners) const; + void _update_rvo_simulation(); + void _update_rvo_obstacles_tree_2d(); + void _update_rvo_agents_tree_2d(); + void _update_rvo_agents_tree_3d(); }; #endif // NAV_MAP_H diff --git a/modules/navigation/nav_obstacle.cpp b/modules/navigation/nav_obstacle.cpp new file mode 100644 index 00000000000..5d0bc59cbb2 --- /dev/null +++ b/modules/navigation/nav_obstacle.cpp @@ -0,0 +1,86 @@ +/**************************************************************************/ +/* nav_obstacle.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#include "nav_obstacle.h" + +#include "nav_map.h" + +NavObstacle::NavObstacle() {} + +NavObstacle::~NavObstacle() {} + +void NavObstacle::set_map(NavMap *p_map) { + if (map != p_map) { + map = p_map; + obstacle_dirty = true; + } +} + +void NavObstacle::set_position(const Vector3 p_position) { + if (position != p_position) { + position = p_position; + obstacle_dirty = true; + } +} + +void NavObstacle::set_height(const real_t p_height) { + if (height != p_height) { + height = p_height; + obstacle_dirty = true; + } +} + +void NavObstacle::set_vertices(const Vector &p_vertices) { + if (vertices != p_vertices) { + vertices = p_vertices; + obstacle_dirty = true; + } +} + +bool NavObstacle::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 NavObstacle::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + obstacle_dirty = true; +} + +bool NavObstacle::check_dirty() { + const bool was_dirty = obstacle_dirty; + obstacle_dirty = false; + return was_dirty; +} diff --git a/modules/navigation/nav_obstacle.h b/modules/navigation/nav_obstacle.h new file mode 100644 index 00000000000..f59eba5200d --- /dev/null +++ b/modules/navigation/nav_obstacle.h @@ -0,0 +1,78 @@ +/**************************************************************************/ +/* nav_obstacle.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#ifndef NAV_OBSTACLE_H +#define NAV_OBSTACLE_H + +#include "core/object/class_db.h" +#include "core/templates/local_vector.h" +#include "nav_agent.h" +#include "nav_rid.h" + +class NavMap; + +class NavObstacle : public NavRid { + NavMap *map = nullptr; + Vector3 position; + Vector vertices; + + real_t height = 0.0; + + uint32_t avoidance_layers = 1; + + bool obstacle_dirty = true; + + uint32_t map_update_id = 0; + +public: + NavObstacle(); + ~NavObstacle(); + + void set_map(NavMap *p_map); + NavMap *get_map() { return map; } + + void set_position(const Vector3 p_position); + const Vector3 &get_position() const { return position; } + + void set_height(const real_t p_height); + real_t get_height() const { return height; } + + void set_vertices(const Vector &p_vertices); + const Vector &get_vertices() const { return vertices; } + + bool is_map_changed(); + + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const { return avoidance_layers; }; + + bool check_dirty(); +}; + +#endif // NAV_OBSTACLE_H diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp index d101cac4ebd..936244fdb14 100644 --- a/scene/2d/navigation_agent_2d.cpp +++ b/scene/2d/navigation_agent_2d.cpp @@ -56,8 +56,11 @@ void NavigationAgent2D::_bind_methods() { 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_time_horizon_agents", "time_horizon"), &NavigationAgent2D::set_time_horizon_agents); + ClassDB::bind_method(D_METHOD("get_time_horizon_agents"), &NavigationAgent2D::get_time_horizon_agents); + + ClassDB::bind_method(D_METHOD("set_time_horizon_obstacles", "time_horizon"), &NavigationAgent2D::set_time_horizon_obstacles); + ClassDB::bind_method(D_METHOD("get_time_horizon_obstacles"), &NavigationAgent2D::get_time_horizon_obstacles); 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); @@ -87,9 +90,16 @@ void NavigationAgent2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_target_position"), &NavigationAgent2D::get_target_position); ClassDB::bind_method(D_METHOD("get_next_path_position"), &NavigationAgent2D::get_next_path_position); - ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target); + + ClassDB::bind_method(D_METHOD("set_velocity_forced", "velocity"), &NavigationAgent2D::set_velocity_forced); + ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent2D::set_velocity); + ClassDB::bind_method(D_METHOD("get_velocity"), &NavigationAgent2D::get_velocity); + + ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target); + ClassDB::bind_method(D_METHOD("get_current_navigation_result"), &NavigationAgent2D::get_current_navigation_result); + ClassDB::bind_method(D_METHOD("get_current_navigation_path"), &NavigationAgent2D::get_current_navigation_path); ClassDB::bind_method(D_METHOD("get_current_navigation_path_index"), &NavigationAgent2D::get_current_navigation_path_index); ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent2D::is_target_reached); @@ -99,6 +109,17 @@ void NavigationAgent2D::_bind_methods() { ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent2D::_avoidance_done); + ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationAgent2D::set_avoidance_layers); + ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationAgent2D::get_avoidance_layers); + ClassDB::bind_method(D_METHOD("set_avoidance_mask", "mask"), &NavigationAgent2D::set_avoidance_mask); + ClassDB::bind_method(D_METHOD("get_avoidance_mask"), &NavigationAgent2D::get_avoidance_mask); + ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationAgent2D::set_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationAgent2D::get_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("set_avoidance_mask_value", "mask_number", "value"), &NavigationAgent2D::set_avoidance_mask_value); + ClassDB::bind_method(D_METHOD("get_avoidance_mask_value", "mask_number"), &NavigationAgent2D::get_avoidance_mask_value); + ClassDB::bind_method(D_METHOD("set_avoidance_priority", "priority"), &NavigationAgent2D::set_avoidance_priority); + ClassDB::bind_method(D_METHOD("get_avoidance_priority"), &NavigationAgent2D::get_avoidance_priority); + ADD_GROUP("Pathfinding", ""); ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "target_position", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_target_position", "get_target_position"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_desired_distance", PROPERTY_HINT_RANGE, "0.1,1000,0.01,or_greater,suffix:px"), "set_path_desired_distance", "get_path_desired_distance"); @@ -111,11 +132,16 @@ void NavigationAgent2D::_bind_methods() { ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.1,500,0.01,or_greater,suffix:px"), "set_radius", "get_radius"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "velocity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_velocity", "get_velocity"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.01,500,0.01,or_greater,suffix:px"), "set_radius", "get_radius"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "neighbor_distance", PROPERTY_HINT_RANGE, "0.1,100000,0.01,or_greater,suffix:px"), "set_neighbor_distance", "get_neighbor_distance"); - ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,or_greater,1"), "set_max_neighbors", "get_max_neighbors"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "time_horizon", PROPERTY_HINT_RANGE, "0.1,10,0.01,or_greater,suffix:s"), "set_time_horizon", "get_time_horizon"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "max_speed", PROPERTY_HINT_RANGE, "0.1,10000,0.01,or_greater,suffix:px/s"), "set_max_speed", "get_max_speed"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1,or_greater"), "set_max_neighbors", "get_max_neighbors"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "time_horizon_agents", PROPERTY_HINT_RANGE, "0.0,10,0.01,or_greater,suffix:s"), "set_time_horizon_agents", "get_time_horizon_agents"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "time_horizon_obstacles", PROPERTY_HINT_RANGE, "0.0,10,0.01,or_greater,suffix:s"), "set_time_horizon_obstacles", "get_time_horizon_obstacles"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "max_speed", PROPERTY_HINT_RANGE, "0.01,100000,0.01,or_greater,suffix:px/s"), "set_max_speed", "get_max_speed"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_mask", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_mask", "get_avoidance_mask"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "avoidance_priority", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_avoidance_priority", "get_avoidance_priority"); ClassDB::bind_method(D_METHOD("set_debug_enabled", "enabled"), &NavigationAgent2D::set_debug_enabled); ClassDB::bind_method(D_METHOD("get_debug_enabled"), &NavigationAgent2D::get_debug_enabled); @@ -143,6 +169,34 @@ void NavigationAgent2D::_bind_methods() { ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR2, "safe_velocity"))); } +#ifndef DISABLE_DEPRECATED +// Compatibility with Godot 4.0 beta 10 or below. +// Functions in block below all renamed or replaced in 4.0 beta 1X avoidance rework. +bool NavigationAgent2D::_set(const StringName &p_name, const Variant &p_value) { + if (p_name == "time_horizon") { + set_time_horizon_agents(p_value); + return true; + } + if (p_name == "target_location") { + set_target_position(p_value); + return true; + } + return false; +} + +bool NavigationAgent2D::_get(const StringName &p_name, Variant &r_ret) const { + if (p_name == "time_horizon") { + r_ret = get_time_horizon_agents(); + return true; + } + if (p_name == "target_location") { + r_ret = get_target_position(); + return true; + } + return false; +} +#endif // DISABLE_DEPRECATED + void NavigationAgent2D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_POST_ENTER_TREE: { @@ -151,11 +205,16 @@ void NavigationAgent2D::_notification(int p_what) { set_agent_parent(get_parent()); set_physics_process_internal(true); + if (agent_parent && avoidance_enabled) { + NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position()); + } + #ifdef DEBUG_ENABLED if (NavigationServer2D::get_singleton()->get_debug_enabled()) { debug_path_dirty = true; } #endif // DEBUG_ENABLED + } break; case NOTIFICATION_PARENTED: { @@ -208,10 +267,18 @@ void NavigationAgent2D::_notification(int p_what) { case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { if (agent_parent && target_position_submitted) { - if (avoidance_enabled) { - // agent_position on NavigationServer is avoidance only and has nothing to do with pathfinding - // no point in flooding NavigationServer queue with agent position updates that get send to the void if avoidance is not used - NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position()); + if (velocity_submitted) { + velocity_submitted = false; + if (avoidance_enabled) { + NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position()); + NavigationServer2D::get_singleton()->agent_set_velocity(agent, velocity); + } + } + if (velocity_forced_submitted) { + velocity_forced_submitted = false; + if (avoidance_enabled) { + NavigationServer2D::get_singleton()->agent_set_velocity_forced(agent, velocity_forced); + } } _check_distance_to_target(); } @@ -226,9 +293,11 @@ void NavigationAgent2D::_notification(int p_what) { NavigationAgent2D::NavigationAgent2D() { agent = NavigationServer2D::get_singleton()->agent_create(); + NavigationServer2D::get_singleton()->agent_set_neighbor_distance(agent, neighbor_distance); NavigationServer2D::get_singleton()->agent_set_max_neighbors(agent, max_neighbors); - NavigationServer2D::get_singleton()->agent_set_time_horizon(agent, time_horizon); + NavigationServer2D::get_singleton()->agent_set_time_horizon_agents(agent, time_horizon_agents); + NavigationServer2D::get_singleton()->agent_set_time_horizon_obstacles(agent, time_horizon_obstacles); NavigationServer2D::get_singleton()->agent_set_radius(agent, radius); NavigationServer2D::get_singleton()->agent_set_max_speed(agent, max_speed); @@ -239,6 +308,11 @@ NavigationAgent2D::NavigationAgent2D() { navigation_result = Ref(); navigation_result.instantiate(); + set_avoidance_layers(avoidance_layers); + set_avoidance_mask(avoidance_mask); + set_avoidance_priority(avoidance_priority); + set_avoidance_enabled(avoidance_enabled); + #ifdef DEBUG_ENABLED NavigationServer2D::get_singleton()->connect(SNAME("navigation_debug_changed"), callable_mp(this, &NavigationAgent2D::_navigation_debug_changed)); #endif // DEBUG_ENABLED @@ -267,9 +341,11 @@ void NavigationAgent2D::set_avoidance_enabled(bool p_enabled) { avoidance_enabled = p_enabled; if (avoidance_enabled) { - NavigationServer2D::get_singleton()->agent_set_callback(agent, callable_mp(this, &NavigationAgent2D::_avoidance_done)); + NavigationServer2D::get_singleton()->agent_set_avoidance_enabled(agent, true); + NavigationServer2D::get_singleton()->agent_set_avoidance_callback(agent, callable_mp(this, &NavigationAgent2D::_avoidance_done)); } else { - NavigationServer2D::get_singleton()->agent_set_callback(agent, Callable()); + NavigationServer2D::get_singleton()->agent_set_avoidance_enabled(agent, false); + NavigationServer2D::get_singleton()->agent_set_avoidance_callback(agent, Callable()); } } @@ -283,7 +359,7 @@ void NavigationAgent2D::set_agent_parent(Node *p_agent_parent) { } // remove agent from any avoidance map before changing parent or there will be leftovers on the RVO map - NavigationServer2D::get_singleton()->agent_set_callback(agent, Callable()); + NavigationServer2D::get_singleton()->agent_set_avoidance_callback(agent, Callable()); if (Object::cast_to(p_agent_parent) != nullptr) { // place agent on navigation map first or else the RVO agent callback creation fails silently later @@ -296,7 +372,7 @@ void NavigationAgent2D::set_agent_parent(Node *p_agent_parent) { // create new avoidance callback if enabled if (avoidance_enabled) { - NavigationServer2D::get_singleton()->agent_set_callback(agent, callable_mp(this, &NavigationAgent2D::_avoidance_done)); + NavigationServer2D::get_singleton()->agent_set_avoidance_callback(agent, callable_mp(this, &NavigationAgent2D::_avoidance_done)); } } else { agent_parent = nullptr; @@ -401,6 +477,7 @@ void NavigationAgent2D::set_target_desired_distance(real_t p_target_desired_dist } void NavigationAgent2D::set_radius(real_t p_radius) { + ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive."); if (Math::is_equal_approx(radius, p_radius)) { return; } @@ -430,21 +507,29 @@ void NavigationAgent2D::set_max_neighbors(int p_count) { NavigationServer2D::get_singleton()->agent_set_max_neighbors(agent, max_neighbors); } -void NavigationAgent2D::set_time_horizon(real_t p_time) { - if (Math::is_equal_approx(time_horizon, p_time)) { +void NavigationAgent2D::set_time_horizon_agents(real_t p_time_horizon) { + ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive."); + if (Math::is_equal_approx(time_horizon_agents, p_time_horizon)) { return; } + time_horizon_agents = p_time_horizon; + NavigationServer2D::get_singleton()->agent_set_time_horizon_agents(agent, time_horizon_agents); +} - time_horizon = p_time; - - NavigationServer2D::get_singleton()->agent_set_time_horizon(agent, time_horizon); +void NavigationAgent2D::set_time_horizon_obstacles(real_t p_time_horizon) { + ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive."); + if (Math::is_equal_approx(time_horizon_obstacles, p_time_horizon)) { + return; + } + time_horizon_obstacles = p_time_horizon; + NavigationServer2D::get_singleton()->agent_set_time_horizon_obstacles(agent, time_horizon_obstacles); } void NavigationAgent2D::set_max_speed(real_t p_max_speed) { + ERR_FAIL_COND_MSG(p_max_speed < 0.0, "Max speed must be positive."); if (Math::is_equal_approx(max_speed, p_max_speed)) { return; } - max_speed = p_max_speed; NavigationServer2D::get_singleton()->agent_set_max_speed(agent, max_speed); @@ -516,29 +601,24 @@ Vector2 NavigationAgent2D::get_final_position() { return navigation_path[navigation_path.size() - 1]; } -void NavigationAgent2D::set_velocity(Vector2 p_velocity) { +void NavigationAgent2D::set_velocity_forced(Vector2 p_velocity) { // Intentionally not checking for equality of the parameter. // We need to always submit the velocity to the navigation server, even when it is the same, in order to run avoidance every frame. // Revisit later when the navigation server can update avoidance without users resubmitting the velocity. - target_velocity = p_velocity; - velocity_submitted = true; + velocity_forced = p_velocity; + velocity_forced_submitted = true; +} - NavigationServer2D::get_singleton()->agent_set_target_velocity(agent, target_velocity); - NavigationServer2D::get_singleton()->agent_set_velocity(agent, prev_safe_velocity); +void NavigationAgent2D::set_velocity(const Vector2 p_velocity) { + velocity = p_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(SNAME("velocity_computed"), velocity); + const Vector2 new_safe_velocity = Vector2(p_new_velocity.x, p_new_velocity.z); + safe_velocity = new_safe_velocity; + emit_signal(SNAME("velocity_computed"), safe_velocity); } PackedStringArray NavigationAgent2D::get_configuration_warnings() const { @@ -561,9 +641,6 @@ void NavigationAgent2D::update_navigation() { if (!target_position_submitted) { return; } - if (update_frame_id == Engine::get_singleton()->get_physics_frames()) { - return; - } update_frame_id = Engine::get_singleton()->get_physics_frames(); @@ -709,6 +786,71 @@ void NavigationAgent2D::_check_distance_to_target() { } } +void NavigationAgent2D::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + NavigationServer2D::get_singleton()->agent_set_avoidance_layers(get_rid(), avoidance_layers); +} + +uint32_t NavigationAgent2D::get_avoidance_layers() const { + return avoidance_layers; +} + +void NavigationAgent2D::set_avoidance_mask(uint32_t p_mask) { + avoidance_mask = p_mask; + NavigationServer2D::get_singleton()->agent_set_avoidance_mask(get_rid(), p_mask); +} + +uint32_t NavigationAgent2D::get_avoidance_mask() const { + return avoidance_mask; +} + +void NavigationAgent2D::set_avoidance_layer_value(int p_layer_number, bool p_value) { + ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive."); + uint32_t avoidance_layers_new = get_avoidance_layers(); + if (p_value) { + avoidance_layers_new |= 1 << (p_layer_number - 1); + } else { + avoidance_layers_new &= ~(1 << (p_layer_number - 1)); + } + set_avoidance_layers(avoidance_layers_new); +} + +bool NavigationAgent2D::get_avoidance_layer_value(int p_layer_number) const { + ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive."); + return get_avoidance_layers() & (1 << (p_layer_number - 1)); +} + +void NavigationAgent2D::set_avoidance_mask_value(int p_mask_number, bool p_value) { + ERR_FAIL_COND_MSG(p_mask_number < 1, "Avoidance mask number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_mask_number > 32, "Avoidance mask number must be between 1 and 32 inclusive."); + uint32_t mask = get_avoidance_mask(); + if (p_value) { + mask |= 1 << (p_mask_number - 1); + } else { + mask &= ~(1 << (p_mask_number - 1)); + } + set_avoidance_mask(mask); +} + +bool NavigationAgent2D::get_avoidance_mask_value(int p_mask_number) const { + ERR_FAIL_COND_V_MSG(p_mask_number < 1, false, "Avoidance mask number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_mask_number > 32, false, "Avoidance mask number must be between 1 and 32 inclusive."); + return get_avoidance_mask() & (1 << (p_mask_number - 1)); +} + +void NavigationAgent2D::set_avoidance_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + avoidance_priority = p_priority; + NavigationServer2D::get_singleton()->agent_set_avoidance_priority(get_rid(), p_priority); +} + +real_t NavigationAgent2D::get_avoidance_priority() const { + return avoidance_priority; +} + ////////DEBUG//////////////////////////////////////////////////////////// void NavigationAgent2D::set_debug_enabled(bool p_enabled) { diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h index b7febba89d0..ca43abf8331 100644 --- a/scene/2d/navigation_agent_2d.h +++ b/scene/2d/navigation_agent_2d.h @@ -47,6 +47,9 @@ class NavigationAgent2D : public Node { RID map_override; bool avoidance_enabled = false; + uint32_t avoidance_layers = 1; + uint32_t avoidance_mask = 1; + real_t avoidance_priority = 1.0; uint32_t navigation_layers = 1; NavigationPathQueryParameters2D::PathfindingAlgorithm pathfinding_algorithm = NavigationPathQueryParameters2D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; NavigationPathQueryParameters2D::PathPostProcessing path_postprocessing = NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; @@ -57,19 +60,33 @@ class NavigationAgent2D : public Node { real_t radius = 10.0; real_t neighbor_distance = 500.0; int max_neighbors = 10; - real_t time_horizon = 1.0; + real_t time_horizon_agents = 1.0; + real_t time_horizon_obstacles = 0.0; real_t max_speed = 100.0; real_t path_max_distance = 100.0; Vector2 target_position; - bool target_position_submitted = false; + Ref navigation_query; Ref navigation_result; int navigation_path_index = 0; + + // the velocity result of the avoidance simulation step + Vector2 safe_velocity; + + /// The submitted target velocity, sets the "wanted" rvo agent velocity on the next update + // this velocity is not guaranteed, the simulation will try to fulfil it if possible + // if other agents or obstacles interfere it will be changed accordingly + Vector2 velocity; bool velocity_submitted = false; - Vector2 prev_safe_velocity; - /// The submitted target velocity - Vector2 target_velocity; + + /// The submitted forced velocity, overrides the rvo agent velocity on the next update + // should only be used very intentionally and not every frame as it interferes with the simulation stability + Vector2 velocity_forced; + bool velocity_forced_submitted = false; + + bool target_position_submitted = false; + bool target_reached = false; bool navigation_finished = true; // No initialized on purpose @@ -81,27 +98,27 @@ class NavigationAgent2D : public Node { float debug_path_custom_line_width = -1.0; bool debug_use_custom = false; Color debug_path_custom_color = Color(1.0, 1.0, 1.0, 1.0); + #ifdef DEBUG_ENABLED // Debug properties internal only bool debug_path_dirty = true; RID debug_path_instance; - -private: - void _navigation_debug_changed(); - void _update_debug_path(); #endif // DEBUG_ENABLED protected: static void _bind_methods(); void _notification(int p_what); +#ifndef DISABLE_DEPRECATED + bool _set(const StringName &p_name, const Variant &p_value); + bool _get(const StringName &p_name, Variant &r_ret) const; +#endif // DISABLE_DEPRECATED + public: NavigationAgent2D(); virtual ~NavigationAgent2D(); - RID get_rid() const { - return agent; - } + RID get_rid() const { return agent; } void set_avoidance_enabled(bool p_enabled); bool get_avoidance_enabled() const; @@ -133,39 +150,28 @@ public: RID get_navigation_map() const; void set_path_desired_distance(real_t p_dd); - real_t get_path_desired_distance() const { - return path_desired_distance; - } + real_t get_path_desired_distance() const { return path_desired_distance; } void set_target_desired_distance(real_t p_dd); - real_t get_target_desired_distance() const { - return target_desired_distance; - } + 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; - } + real_t get_radius() const { return radius; } void set_neighbor_distance(real_t p_distance); - real_t get_neighbor_distance() const { - return neighbor_distance; - } + real_t get_neighbor_distance() const { return neighbor_distance; } void set_max_neighbors(int p_count); - int get_max_neighbors() const { - return max_neighbors; - } + 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_time_horizon_agents(real_t p_time_horizon); + real_t get_time_horizon_agents() const { return time_horizon_agents; } + + void set_time_horizon_obstacles(real_t p_time_horizon); + real_t get_time_horizon_obstacles() const { return time_horizon_obstacles; } void set_max_speed(real_t p_max_speed); - real_t get_max_speed() const { - return 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(); @@ -175,15 +181,11 @@ public: Vector2 get_next_path_position(); - Ref get_current_navigation_result() const { - return navigation_result; - } - const Vector &get_current_navigation_path() const { - return navigation_result->get_path(); - } - int get_current_navigation_path_index() const { - return navigation_path_index; - } + Ref get_current_navigation_result() const { return navigation_result; } + + const Vector &get_current_navigation_path() const { return navigation_result->get_path(); } + + int get_current_navigation_path_index() const { return navigation_path_index; } real_t distance_to_target() const; bool is_target_reached() const; @@ -191,11 +193,30 @@ public: bool is_navigation_finished(); Vector2 get_final_position(); - void set_velocity(Vector2 p_velocity); + void set_velocity(const Vector2 p_velocity); + Vector2 get_velocity() { return velocity; } + + void set_velocity_forced(const Vector2 p_velocity); + void _avoidance_done(Vector3 p_new_velocity); PackedStringArray get_configuration_warnings() const override; + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const; + + void set_avoidance_mask(uint32_t p_mask); + uint32_t get_avoidance_mask() const; + + void set_avoidance_layer_value(int p_layer_number, bool p_value); + bool get_avoidance_layer_value(int p_layer_number) const; + + void set_avoidance_mask_value(int p_mask_number, bool p_value); + bool get_avoidance_mask_value(int p_mask_number) const; + + void set_avoidance_priority(real_t p_priority); + real_t get_avoidance_priority() const; + void set_debug_enabled(bool p_enabled); bool get_debug_enabled() const; @@ -215,6 +236,11 @@ private: void update_navigation(); void _request_repath(); void _check_distance_to_target(); + +#ifdef DEBUG_ENABLED + void _navigation_debug_changed(); + void _update_debug_path(); +#endif // DEBUG_ENABLED }; #endif // NAVIGATION_AGENT_2D_H diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp index 2366cb696ef..d368a4d4369 100644 --- a/scene/2d/navigation_obstacle_2d.cpp +++ b/scene/2d/navigation_obstacle_2d.cpp @@ -30,223 +30,280 @@ #include "navigation_obstacle_2d.h" -#include "scene/2d/collision_shape_2d.h" -#include "scene/2d/physics_body_2d.h" +#include "core/math/geometry_2d.h" #include "scene/resources/world_2d.h" #include "servers/navigation_server_2d.h" +#include "servers/navigation_server_3d.h" void NavigationObstacle2D::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle2D::get_rid); + ClassDB::bind_method(D_METHOD("get_obstacle_rid"), &NavigationObstacle2D::get_obstacle_rid); + ClassDB::bind_method(D_METHOD("get_agent_rid"), &NavigationObstacle2D::get_agent_rid); ClassDB::bind_method(D_METHOD("set_navigation_map", "navigation_map"), &NavigationObstacle2D::set_navigation_map); ClassDB::bind_method(D_METHOD("get_navigation_map"), &NavigationObstacle2D::get_navigation_map); - ClassDB::bind_method(D_METHOD("set_estimate_radius", "estimate_radius"), &NavigationObstacle2D::set_estimate_radius); - ClassDB::bind_method(D_METHOD("is_radius_estimated"), &NavigationObstacle2D::is_radius_estimated); ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationObstacle2D::set_radius); ClassDB::bind_method(D_METHOD("get_radius"), &NavigationObstacle2D::get_radius); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "estimate_radius"), "set_estimate_radius", "is_radius_estimated"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.01,500,0.01"), "set_radius", "get_radius"); -} + ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationObstacle2D::set_velocity); + ClassDB::bind_method(D_METHOD("get_velocity"), &NavigationObstacle2D::get_velocity); -void NavigationObstacle2D::_validate_property(PropertyInfo &p_property) const { - if (p_property.name == "radius") { - if (estimate_radius) { - p_property.usage = PROPERTY_USAGE_NO_EDITOR; - } - } + ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationObstacle2D::set_vertices); + ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationObstacle2D::get_vertices); + + ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationObstacle2D::set_avoidance_layers); + ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationObstacle2D::get_avoidance_layers); + ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationObstacle2D::set_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationObstacle2D::get_avoidance_layer_value); + + ADD_GROUP("Avoidance", "avoidance_"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "velocity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_velocity", "get_velocity"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.0,500,0.01,suffix:px"), "set_radius", "get_radius"); + ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR2_ARRAY, "vertices"), "set_vertices", "get_vertices"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers"); } void NavigationObstacle2D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_POST_ENTER_TREE: { - set_agent_parent(get_parent()); + if (map_override.is_valid()) { + _update_map(map_override); + } else if (is_inside_tree()) { + _update_map(get_world_2d()->get_navigation_map()); + } else { + _update_map(RID()); + } + previous_transform = get_global_transform(); + // need to trigger map controlled agent assignment somehow for the fake_agent since obstacles use no callback like regular agents + NavigationServer2D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0); + _update_position(get_global_transform().get_origin()); set_physics_process_internal(true); } break; case NOTIFICATION_EXIT_TREE: { - set_agent_parent(nullptr); - set_physics_process_internal(false); - } break; - - case NOTIFICATION_PARENTED: { - if (is_inside_tree() && (get_parent() != parent_node2d)) { - set_agent_parent(get_parent()); - set_physics_process_internal(true); - } - } break; - - case NOTIFICATION_UNPARENTED: { - set_agent_parent(nullptr); set_physics_process_internal(false); + _update_map(RID()); } break; case NOTIFICATION_PAUSED: { - if (parent_node2d && !parent_node2d->can_process()) { - map_before_pause = NavigationServer2D::get_singleton()->agent_get_map(get_rid()); - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID()); - } else if (parent_node2d && parent_node2d->can_process() && !(map_before_pause == RID())) { - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_before_pause); + if (!can_process()) { + map_before_pause = map_current; + _update_map(RID()); + } else if (can_process() && !(map_before_pause == RID())) { + _update_map(map_before_pause); map_before_pause = RID(); } } break; case NOTIFICATION_UNPAUSED: { - if (parent_node2d && !parent_node2d->can_process()) { - map_before_pause = NavigationServer2D::get_singleton()->agent_get_map(get_rid()); - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID()); - } else if (parent_node2d && parent_node2d->can_process() && !(map_before_pause == RID())) { - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_before_pause); + if (!can_process()) { + map_before_pause = map_current; + _update_map(RID()); + } else if (can_process() && !(map_before_pause == RID())) { + _update_map(map_before_pause); map_before_pause = RID(); } } break; case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { - if (parent_node2d && parent_node2d->is_inside_tree()) { - NavigationServer2D::get_singleton()->agent_set_position(agent, parent_node2d->get_global_position()); + if (is_inside_tree()) { + _update_position(get_global_transform().get_origin()); + + if (velocity_submitted) { + velocity_submitted = false; + // only update if there is a noticeable change, else the rvo agent preferred velocity stays the same + if (!previous_velocity.is_equal_approx(velocity)) { + NavigationServer2D::get_singleton()->agent_set_velocity(fake_agent, velocity); + } + previous_velocity = velocity; + } } } break; + + case NOTIFICATION_DRAW: { +#ifdef DEBUG_ENABLED + if (is_inside_tree()) { + bool is_debug_enabled = false; + if (Engine::get_singleton()->is_editor_hint()) { + is_debug_enabled = true; + } else if (NavigationServer2D::get_singleton()->get_debug_enabled() && NavigationServer2D::get_singleton()->get_debug_avoidance_enabled()) { + is_debug_enabled = true; + } + + if (is_debug_enabled) { + _update_fake_agent_radius_debug(); + _update_static_obstacle_debug(); + } + } +#endif // DEBUG_ENABLED + } break; } } NavigationObstacle2D::NavigationObstacle2D() { - agent = NavigationServer2D::get_singleton()->agent_create(); - initialize_agent(); + obstacle = NavigationServer2D::get_singleton()->obstacle_create(); + fake_agent = NavigationServer2D::get_singleton()->agent_create(); + + // change properties of the fake agent so it can act as fake obstacle with a radius + NavigationServer2D::get_singleton()->agent_set_neighbor_distance(fake_agent, 0.0); + NavigationServer2D::get_singleton()->agent_set_max_neighbors(fake_agent, 0); + NavigationServer2D::get_singleton()->agent_set_time_horizon_agents(fake_agent, 0.0); + NavigationServer2D::get_singleton()->agent_set_time_horizon_obstacles(fake_agent, 0.0); + NavigationServer2D::get_singleton()->agent_set_max_speed(fake_agent, 0.0); + NavigationServer2D::get_singleton()->agent_set_avoidance_mask(fake_agent, 0); + NavigationServer2D::get_singleton()->agent_set_avoidance_priority(fake_agent, 1.0); + NavigationServer2D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0); + + set_radius(radius); + set_vertices(vertices); + set_avoidance_layers(avoidance_layers); } NavigationObstacle2D::~NavigationObstacle2D() { ERR_FAIL_NULL(NavigationServer2D::get_singleton()); - NavigationServer2D::get_singleton()->free(agent); - agent = RID(); // Pointless + + NavigationServer2D::get_singleton()->free(obstacle); + obstacle = RID(); + + NavigationServer2D::get_singleton()->free(fake_agent); + fake_agent = RID(); } -PackedStringArray NavigationObstacle2D::get_configuration_warnings() const { - PackedStringArray warnings = Node::get_configuration_warnings(); - - if (!Object::cast_to(get_parent())) { - warnings.push_back(RTR("The NavigationObstacle2D only serves to provide collision avoidance to a Node2D object.")); +void NavigationObstacle2D::set_vertices(const Vector &p_vertices) { + vertices = p_vertices; + NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle, vertices); + if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint())) { + queue_redraw(); } - - if (Object::cast_to(get_parent())) { - warnings.push_back(RTR("The NavigationObstacle2D is intended for constantly moving bodies like CharacterBody2D or RigidBody2D as it creates only an RVO avoidance radius and does not follow scene geometry exactly." - "\nNot constantly moving or complete static objects should be captured with a refreshed NavigationPolygon so agents can not only avoid them but also move along those objects outline at high detail")); - } - - return warnings; -} - -void NavigationObstacle2D::initialize_agent() { - NavigationServer2D::get_singleton()->agent_set_neighbor_distance(agent, 0.0); - NavigationServer2D::get_singleton()->agent_set_max_neighbors(agent, 0); - NavigationServer2D::get_singleton()->agent_set_time_horizon(agent, 0.0); - NavigationServer2D::get_singleton()->agent_set_max_speed(agent, 0.0); -} - -void NavigationObstacle2D::reevaluate_agent_radius() { - if (!estimate_radius) { - NavigationServer2D::get_singleton()->agent_set_radius(agent, radius); - } else if (parent_node2d && parent_node2d->is_inside_tree()) { - NavigationServer2D::get_singleton()->agent_set_radius(agent, estimate_agent_radius()); - } -} - -real_t NavigationObstacle2D::estimate_agent_radius() const { - if (parent_node2d && parent_node2d->is_inside_tree()) { - // Estimate the radius of this physics body - real_t max_radius = 0.0; - for (int i(0); i < parent_node2d->get_child_count(); i++) { - // For each collision shape - CollisionShape2D *cs = Object::cast_to(parent_node2d->get_child(i)); - if (cs && cs->is_inside_tree()) { - // 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_scale(); - r *= MAX(s.x, s.y); - // Takes the biggest radius - max_radius = MAX(max_radius, r); - } else if (cs && !cs->is_inside_tree()) { - WARN_PRINT("A CollisionShape2D of the NavigationObstacle2D parent node was not inside the SceneTree when estimating the obstacle radius." - "\nMove the NavigationObstacle2D to a child position below any CollisionShape2D node of the parent node so the CollisionShape2D is already inside the SceneTree."); - } - } - Vector2 s = parent_node2d->get_global_scale(); - max_radius *= MAX(s.x, s.y); - - if (max_radius > 0.0) { - return max_radius; - } - } - return 1.0; // Never a 0 radius -} - -void NavigationObstacle2D::set_agent_parent(Node *p_agent_parent) { - if (parent_node2d == p_agent_parent) { - return; - } - - if (Object::cast_to(p_agent_parent) != nullptr) { - parent_node2d = Object::cast_to(p_agent_parent); - if (map_override.is_valid()) { - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_override); - } else { - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), parent_node2d->get_world_2d()->get_navigation_map()); - } - // Need to register Callback as obstacle requires a valid Callback to be added to avoidance simulation. - NavigationServer2D::get_singleton()->agent_set_callback(get_rid(), callable_mp(this, &NavigationObstacle2D::_avoidance_done)); - reevaluate_agent_radius(); - } else { - parent_node2d = nullptr; - NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID()); - NavigationServer2D::get_singleton()->agent_set_callback(agent, Callable()); - } -} - -void NavigationObstacle2D::_avoidance_done(Vector3 p_new_velocity) { - // Dummy function as obstacle requires a valid Callback to be added to avoidance simulation. } void NavigationObstacle2D::set_navigation_map(RID p_navigation_map) { if (map_override == p_navigation_map) { return; } - map_override = p_navigation_map; - - NavigationServer2D::get_singleton()->agent_set_map(agent, map_override); + _update_map(map_override); } RID NavigationObstacle2D::get_navigation_map() const { if (map_override.is_valid()) { return map_override; - } else if (parent_node2d != nullptr) { - return parent_node2d->get_world_2d()->get_navigation_map(); + } else if (is_inside_tree()) { + return get_world_2d()->get_navigation_map(); } return RID(); } -void NavigationObstacle2D::set_estimate_radius(bool p_estimate_radius) { - if (estimate_radius == p_estimate_radius) { - return; - } - - estimate_radius = p_estimate_radius; - - notify_property_list_changed(); - reevaluate_agent_radius(); -} - void NavigationObstacle2D::set_radius(real_t p_radius) { - ERR_FAIL_COND_MSG(p_radius <= 0.0, "Radius must be greater than 0."); + ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive."); if (Math::is_equal_approx(radius, p_radius)) { return; } radius = p_radius; - reevaluate_agent_radius(); + NavigationServer2D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0.0); + NavigationServer2D::get_singleton()->agent_set_radius(fake_agent, radius); + if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint())) { + queue_redraw(); + } } + +void NavigationObstacle2D::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + NavigationServer2D::get_singleton()->obstacle_set_avoidance_layers(obstacle, avoidance_layers); + NavigationServer2D::get_singleton()->agent_set_avoidance_layers(fake_agent, avoidance_layers); +} + +uint32_t NavigationObstacle2D::get_avoidance_layers() const { + return avoidance_layers; +} + +void NavigationObstacle2D::set_avoidance_layer_value(int p_layer_number, bool p_value) { + ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive."); + uint32_t avoidance_layers_new = get_avoidance_layers(); + if (p_value) { + avoidance_layers_new |= 1 << (p_layer_number - 1); + } else { + avoidance_layers_new &= ~(1 << (p_layer_number - 1)); + } + set_avoidance_layers(avoidance_layers_new); +} + +bool NavigationObstacle2D::get_avoidance_layer_value(int p_layer_number) const { + ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive."); + return get_avoidance_layers() & (1 << (p_layer_number - 1)); +} + +void NavigationObstacle2D::set_velocity(const Vector2 p_velocity) { + velocity = p_velocity; + velocity_submitted = true; +} + +void NavigationObstacle2D::_update_map(RID p_map) { + NavigationServer2D::get_singleton()->obstacle_set_map(obstacle, p_map); + NavigationServer2D::get_singleton()->agent_set_map(fake_agent, p_map); + map_current = p_map; +} + +void NavigationObstacle2D::_update_position(const Vector2 p_position) { + if (vertices.size() > 0) { + NavigationServer2D::get_singleton()->obstacle_set_position(obstacle, p_position); + } + if (radius > 0.0) { + NavigationServer2D::get_singleton()->agent_set_position(fake_agent, p_position); + } +} + +#ifdef DEBUG_ENABLED +void NavigationObstacle2D::_update_fake_agent_radius_debug() { + if (radius > 0.0 && NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius()) { + Color debug_radius_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_obstacles_radius_color(); + draw_circle(get_global_transform().get_origin(), radius, debug_radius_color); + } +} +#endif // DEBUG_ENABLED + +#ifdef DEBUG_ENABLED +void NavigationObstacle2D::_update_static_obstacle_debug() { + if (get_vertices().size() > 2 && NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) { + bool obstacle_pushes_inward = Geometry2D::is_polygon_clockwise(get_vertices()); + + Color debug_static_obstacle_face_color; + + if (obstacle_pushes_inward) { + debug_static_obstacle_face_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_color(); + } else { + debug_static_obstacle_face_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_color(); + } + + Vector debug_obstacle_polygon_vertices = get_vertices(); + + Vector debug_obstacle_polygon_colors; + debug_obstacle_polygon_colors.resize(debug_obstacle_polygon_vertices.size()); + debug_obstacle_polygon_colors.fill(debug_static_obstacle_face_color); + + RS::get_singleton()->canvas_item_add_polygon(get_canvas_item(), debug_obstacle_polygon_vertices, debug_obstacle_polygon_colors); + + Color debug_static_obstacle_edge_color; + + if (obstacle_pushes_inward) { + debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_color(); + } else { + debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_color(); + } + + Vector debug_obstacle_line_vertices = get_vertices(); + debug_obstacle_line_vertices.push_back(debug_obstacle_line_vertices[0]); + debug_obstacle_line_vertices.resize(debug_obstacle_line_vertices.size()); + + Vector debug_obstacle_line_colors; + debug_obstacle_line_colors.resize(debug_obstacle_line_vertices.size()); + debug_obstacle_line_colors.fill(debug_static_obstacle_edge_color); + + RS::get_singleton()->canvas_item_add_polyline(get_canvas_item(), debug_obstacle_line_vertices, debug_obstacle_line_colors, 4.0); + } +} +#endif // DEBUG_ENABLED diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h index f856c481b0e..d3b0926247f 100644 --- a/scene/2d/navigation_obstacle_2d.h +++ b/scene/2d/navigation_obstacle_2d.h @@ -32,55 +32,71 @@ #define NAVIGATION_OBSTACLE_2D_H #include "scene/2d/node_2d.h" -#include "scene/main/node.h" -class NavigationObstacle2D : public Node { - GDCLASS(NavigationObstacle2D, Node); +class NavigationObstacle2D : public Node2D { + GDCLASS(NavigationObstacle2D, Node2D); - Node2D *parent_node2d = nullptr; - - RID agent; + RID obstacle; RID map_before_pause; RID map_override; + RID map_current; - bool estimate_radius = true; - real_t radius = 1.0; + real_t radius = 0.0; + + Vector vertices; + + RID fake_agent; + uint32_t avoidance_layers = 1; + + Transform2D previous_transform; + + Vector2 velocity; + Vector2 previous_velocity; + bool velocity_submitted = false; + +#ifdef DEBUG_ENABLED +private: + void _update_fake_agent_radius_debug(); + void _update_static_obstacle_debug(); +#endif // DEBUG_ENABLED protected: static void _bind_methods(); - void _validate_property(PropertyInfo &p_property) const; void _notification(int p_what); public: NavigationObstacle2D(); virtual ~NavigationObstacle2D(); - RID get_rid() const { - return agent; - } - - void set_agent_parent(Node *p_agent_parent); + RID get_obstacle_rid() const { return obstacle; } + RID get_agent_rid() const { return fake_agent; } void set_navigation_map(RID p_navigation_map); RID get_navigation_map() const; - void set_estimate_radius(bool p_estimate_radius); - bool is_radius_estimated() const { - return estimate_radius; - } void set_radius(real_t p_radius); - real_t get_radius() const { - return radius; - } + real_t get_radius() const { return radius; } - PackedStringArray get_configuration_warnings() const override; + void set_vertices(const Vector &p_vertices); + const Vector &get_vertices() const { return vertices; }; + + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const; + + void set_avoidance_mask(uint32_t p_mask); + uint32_t get_avoidance_mask() const; + + void set_avoidance_layer_value(int p_layer_number, bool p_value); + bool get_avoidance_layer_value(int p_layer_number) const; + + void set_velocity(const Vector2 p_velocity); + Vector2 get_velocity() const { return velocity; }; void _avoidance_done(Vector3 p_new_velocity); // Dummy private: - void initialize_agent(); - void reevaluate_agent_radius(); - real_t estimate_agent_radius() const; + void _update_map(RID p_map); + void _update_position(const Vector2 p_position); }; #endif // NAVIGATION_OBSTACLE_2D_H diff --git a/scene/2d/navigation_region_2d.cpp b/scene/2d/navigation_region_2d.cpp index 5dbba313bca..edce9c94fde 100644 --- a/scene/2d/navigation_region_2d.cpp +++ b/scene/2d/navigation_region_2d.cpp @@ -31,6 +31,8 @@ #include "navigation_region_2d.h" #include "core/core_string_names.h" +#include "core/math/geometry_2d.h" +#include "scene/2d/navigation_obstacle_2d.h" #include "scene/resources/world_2d.h" #include "servers/navigation_server_2d.h" #include "servers/navigation_server_3d.h" @@ -55,7 +57,7 @@ void NavigationRegion2D::set_enabled(bool p_enabled) { } #ifdef DEBUG_ENABLED - if (Engine::get_singleton()->is_editor_hint() || NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (Engine::get_singleton()->is_editor_hint() || NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { queue_redraw(); } #endif // DEBUG_ENABLED @@ -151,11 +153,22 @@ void NavigationRegion2D::_notification(int p_what) { if (enabled) { NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map()); NavigationServer2D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); + for (uint32_t i = 0; i < constrain_avoidance_obstacles.size(); i++) { + if (constrain_avoidance_obstacles[i].is_valid()) { + NavigationServer2D::get_singleton()->obstacle_set_map(constrain_avoidance_obstacles[i], get_world_2d()->get_navigation_map()); + NavigationServer2D::get_singleton()->obstacle_set_position(constrain_avoidance_obstacles[i], get_global_position()); + } + } } } break; case NOTIFICATION_TRANSFORM_CHANGED: { NavigationServer2D::get_singleton()->region_set_transform(region, get_global_transform()); + for (uint32_t i = 0; i < constrain_avoidance_obstacles.size(); i++) { + if (constrain_avoidance_obstacles[i].is_valid()) { + NavigationServer2D::get_singleton()->obstacle_set_position(constrain_avoidance_obstacles[i], get_global_position()); + } + } } break; case NOTIFICATION_EXIT_TREE: { @@ -163,6 +176,11 @@ void NavigationRegion2D::_notification(int p_what) { if (enabled) { NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); } + for (uint32_t i = 0; i < constrain_avoidance_obstacles.size(); i++) { + if (constrain_avoidance_obstacles[i].is_valid()) { + NavigationServer2D::get_singleton()->obstacle_set_map(constrain_avoidance_obstacles[i], RID()); + } + } } break; case NOTIFICATION_DRAW: { @@ -277,6 +295,7 @@ void NavigationRegion2D::_navigation_polygon_changed() { if (navigation_polygon.is_valid()) { NavigationServer2D::get_singleton()->region_set_navigation_polygon(region, navigation_polygon); } + _update_avoidance_constrain(); } void NavigationRegion2D::_map_changed(RID p_map) { @@ -312,6 +331,13 @@ void NavigationRegion2D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_navigation_layer_value", "layer_number", "value"), &NavigationRegion2D::set_navigation_layer_value); ClassDB::bind_method(D_METHOD("get_navigation_layer_value", "layer_number"), &NavigationRegion2D::get_navigation_layer_value); + ClassDB::bind_method(D_METHOD("set_constrain_avoidance", "enabled"), &NavigationRegion2D::set_constrain_avoidance); + ClassDB::bind_method(D_METHOD("get_constrain_avoidance"), &NavigationRegion2D::get_constrain_avoidance); + ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationRegion2D::set_avoidance_layers); + ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationRegion2D::get_avoidance_layers); + ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationRegion2D::set_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationRegion2D::get_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("get_region_rid"), &NavigationRegion2D::get_region_rid); ClassDB::bind_method(D_METHOD("set_enter_cost", "enter_cost"), &NavigationRegion2D::set_enter_cost); @@ -327,6 +353,8 @@ void NavigationRegion2D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "enter_cost"), "set_enter_cost", "get_enter_cost"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "travel_cost"), "set_travel_cost", "get_travel_cost"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "constrain_avoidance"), "set_constrain_avoidance", "get_constrain_avoidance"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers"); } #ifndef DISABLE_DEPRECATED @@ -367,8 +395,123 @@ NavigationRegion2D::~NavigationRegion2D() { ERR_FAIL_NULL(NavigationServer2D::get_singleton()); NavigationServer2D::get_singleton()->free(region); + for (uint32_t i = 0; i < constrain_avoidance_obstacles.size(); i++) { + if (constrain_avoidance_obstacles[i].is_valid()) { + NavigationServer2D::get_singleton()->free(constrain_avoidance_obstacles[i]); + } + } + constrain_avoidance_obstacles.clear(); + #ifdef DEBUG_ENABLED NavigationServer3D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); NavigationServer3D::get_singleton()->disconnect("navigation_debug_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); #endif // DEBUG_ENABLED } + +void NavigationRegion2D::_update_avoidance_constrain() { + for (uint32_t i = 0; i < constrain_avoidance_obstacles.size(); i++) { + if (constrain_avoidance_obstacles[i].is_valid()) { + NavigationServer2D::get_singleton()->free(constrain_avoidance_obstacles[i]); + constrain_avoidance_obstacles[i] = RID(); + } + } + constrain_avoidance_obstacles.clear(); + + if (!constrain_avoidance) { + return; + } + + if (get_navigation_polygon() == nullptr) { + return; + } + + Ref _navpoly = get_navigation_polygon(); + int _outline_count = _navpoly->get_outline_count(); + if (_outline_count == 0) { + return; + } + + for (int outline_index(0); outline_index < _outline_count; outline_index++) { + const Vector &_outline = _navpoly->get_outline(outline_index); + + const int outline_size = _outline.size(); + if (outline_size < 3) { + ERR_FAIL_COND_MSG(_outline.size() < 3, "NavigationPolygon outline needs to have at least 3 vertex to create avoidance obstacles to constrain avoidance agent's"); + continue; + } + + RID obstacle_rid = NavigationServer2D::get_singleton()->obstacle_create(); + constrain_avoidance_obstacles.push_back(obstacle_rid); + + Vector new_obstacle_outline; + + if (outline_index == 0) { + for (int i(0); i < outline_size; i++) { + new_obstacle_outline.push_back(_outline[outline_size - i - 1]); + } + ERR_FAIL_COND_MSG(Geometry2D::is_polygon_clockwise(_outline), "Outer most outline needs to be clockwise to push avoidance agent inside"); + } else { + for (int i(0); i < outline_size; i++) { + new_obstacle_outline.push_back(_outline[i]); + } + } + new_obstacle_outline.resize(outline_size); + + NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle_rid, new_obstacle_outline); + NavigationServer2D::get_singleton()->obstacle_set_avoidance_layers(obstacle_rid, avoidance_layers); + if (is_inside_tree()) { + NavigationServer2D::get_singleton()->obstacle_set_map(obstacle_rid, get_world_2d()->get_navigation_map()); + NavigationServer2D::get_singleton()->obstacle_set_position(obstacle_rid, get_global_position()); + } + } + constrain_avoidance_obstacles.resize(_outline_count); +} + +void NavigationRegion2D::set_constrain_avoidance(bool p_enabled) { + constrain_avoidance = p_enabled; + _update_avoidance_constrain(); + notify_property_list_changed(); +} + +bool NavigationRegion2D::get_constrain_avoidance() const { + return constrain_avoidance; +} + +void NavigationRegion2D::_validate_property(PropertyInfo &p_property) const { + if (p_property.name == "avoidance_layers") { + if (!constrain_avoidance) { + p_property.usage = PROPERTY_USAGE_NO_EDITOR; + } + } +} + +void NavigationRegion2D::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + if (constrain_avoidance_obstacles.size() > 0) { + for (uint32_t i = 0; i < constrain_avoidance_obstacles.size(); i++) { + NavigationServer2D::get_singleton()->obstacle_set_avoidance_layers(constrain_avoidance_obstacles[i], avoidance_layers); + } + } +} + +uint32_t NavigationRegion2D::get_avoidance_layers() const { + return avoidance_layers; +} + +void NavigationRegion2D::set_avoidance_layer_value(int p_layer_number, bool p_value) { + ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive."); + uint32_t avoidance_layers_new = get_avoidance_layers(); + if (p_value) { + avoidance_layers_new |= 1 << (p_layer_number - 1); + } else { + avoidance_layers_new &= ~(1 << (p_layer_number - 1)); + } + set_avoidance_layers(avoidance_layers_new); +} + +bool NavigationRegion2D::get_avoidance_layer_value(int p_layer_number) const { + ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive."); + return get_avoidance_layers() & (1 << (p_layer_number - 1)); +} diff --git a/scene/2d/navigation_region_2d.h b/scene/2d/navigation_region_2d.h index 49ff143dd8c..fa2edc6112f 100644 --- a/scene/2d/navigation_region_2d.h +++ b/scene/2d/navigation_region_2d.h @@ -43,11 +43,16 @@ class NavigationRegion2D : public Node2D { real_t travel_cost = 1.0; Ref navigation_polygon; + bool constrain_avoidance = false; + LocalVector constrain_avoidance_obstacles; + uint32_t avoidance_layers = 1; + void _navigation_polygon_changed(); void _map_changed(RID p_RID); protected: void _notification(int p_what); + void _validate_property(PropertyInfo &p_property) const; static void _bind_methods(); #ifndef DISABLE_DEPRECATED @@ -81,10 +86,22 @@ public: void set_navigation_polygon(const Ref &p_navigation_polygon); Ref get_navigation_polygon() const; + void set_constrain_avoidance(bool p_enabled); + bool get_constrain_avoidance() const; + + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const; + + void set_avoidance_layer_value(int p_layer_number, bool p_value); + bool get_avoidance_layer_value(int p_layer_number) const; + PackedStringArray get_configuration_warnings() const override; NavigationRegion2D(); ~NavigationRegion2D(); + +private: + void _update_avoidance_constrain(); }; #endif // NAVIGATION_REGION_2D_H diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp index 7d404ffa075..80b360a2fdf 100644 --- a/scene/3d/navigation_agent_3d.cpp +++ b/scene/3d/navigation_agent_3d.cpp @@ -48,11 +48,14 @@ void NavigationAgent3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent3D::set_radius); ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent3D::get_radius); - ClassDB::bind_method(D_METHOD("set_agent_height_offset", "agent_height_offset"), &NavigationAgent3D::set_agent_height_offset); - ClassDB::bind_method(D_METHOD("get_agent_height_offset"), &NavigationAgent3D::get_agent_height_offset); + ClassDB::bind_method(D_METHOD("set_height", "height"), &NavigationAgent3D::set_height); + ClassDB::bind_method(D_METHOD("get_height"), &NavigationAgent3D::get_height); - ClassDB::bind_method(D_METHOD("set_ignore_y", "ignore"), &NavigationAgent3D::set_ignore_y); - ClassDB::bind_method(D_METHOD("get_ignore_y"), &NavigationAgent3D::get_ignore_y); + ClassDB::bind_method(D_METHOD("set_path_height_offset", "path_height_offset"), &NavigationAgent3D::set_path_height_offset); + ClassDB::bind_method(D_METHOD("get_path_height_offset"), &NavigationAgent3D::get_path_height_offset); + + ClassDB::bind_method(D_METHOD("set_use_3d_avoidance", "enabled"), &NavigationAgent3D::set_use_3d_avoidance); + ClassDB::bind_method(D_METHOD("get_use_3d_avoidance"), &NavigationAgent3D::get_use_3d_avoidance); ClassDB::bind_method(D_METHOD("set_neighbor_distance", "neighbor_distance"), &NavigationAgent3D::set_neighbor_distance); ClassDB::bind_method(D_METHOD("get_neighbor_distance"), &NavigationAgent3D::get_neighbor_distance); @@ -60,8 +63,11 @@ void NavigationAgent3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent3D::set_max_neighbors); ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent3D::get_max_neighbors); - ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent3D::set_time_horizon); - ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent3D::get_time_horizon); + ClassDB::bind_method(D_METHOD("set_time_horizon_agents", "time_horizon"), &NavigationAgent3D::set_time_horizon_agents); + ClassDB::bind_method(D_METHOD("get_time_horizon_agents"), &NavigationAgent3D::get_time_horizon_agents); + + ClassDB::bind_method(D_METHOD("set_time_horizon_obstacles", "time_horizon"), &NavigationAgent3D::set_time_horizon_obstacles); + ClassDB::bind_method(D_METHOD("get_time_horizon_obstacles"), &NavigationAgent3D::get_time_horizon_obstacles); ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent3D::set_max_speed); ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent3D::get_max_speed); @@ -91,9 +97,16 @@ void NavigationAgent3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_target_position"), &NavigationAgent3D::get_target_position); ClassDB::bind_method(D_METHOD("get_next_path_position"), &NavigationAgent3D::get_next_path_position); - ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent3D::distance_to_target); + + ClassDB::bind_method(D_METHOD("set_velocity_forced", "velocity"), &NavigationAgent3D::set_velocity_forced); + ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent3D::set_velocity); + ClassDB::bind_method(D_METHOD("get_velocity"), &NavigationAgent3D::get_velocity); + + ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent3D::distance_to_target); + ClassDB::bind_method(D_METHOD("get_current_navigation_result"), &NavigationAgent3D::get_current_navigation_result); + ClassDB::bind_method(D_METHOD("get_current_navigation_path"), &NavigationAgent3D::get_current_navigation_path); ClassDB::bind_method(D_METHOD("get_current_navigation_path_index"), &NavigationAgent3D::get_current_navigation_path_index); ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent3D::is_target_reached); @@ -103,11 +116,22 @@ void NavigationAgent3D::_bind_methods() { ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent3D::_avoidance_done); + ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationAgent3D::set_avoidance_layers); + ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationAgent3D::get_avoidance_layers); + ClassDB::bind_method(D_METHOD("set_avoidance_mask", "mask"), &NavigationAgent3D::set_avoidance_mask); + ClassDB::bind_method(D_METHOD("get_avoidance_mask"), &NavigationAgent3D::get_avoidance_mask); + ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationAgent3D::set_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationAgent3D::get_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("set_avoidance_mask_value", "mask_number", "value"), &NavigationAgent3D::set_avoidance_mask_value); + ClassDB::bind_method(D_METHOD("get_avoidance_mask_value", "mask_number"), &NavigationAgent3D::get_avoidance_mask_value); + ClassDB::bind_method(D_METHOD("set_avoidance_priority", "priority"), &NavigationAgent3D::set_avoidance_priority); + ClassDB::bind_method(D_METHOD("get_avoidance_priority"), &NavigationAgent3D::get_avoidance_priority); + ADD_GROUP("Pathfinding", ""); ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "target_position", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_target_position", "get_target_position"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01,or_greater,suffix:m"), "set_path_desired_distance", "get_path_desired_distance"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01,or_greater,suffix:m"), "set_target_desired_distance", "get_target_desired_distance"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "agent_height_offset", PROPERTY_HINT_RANGE, "-100.0,100,0.01,or_greater,suffix:m"), "set_agent_height_offset", "get_agent_height_offset"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_height_offset", PROPERTY_HINT_RANGE, "-100.0,100,0.01,or_greater,suffix:m"), "set_path_height_offset", "get_path_height_offset"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1,or_greater,suffix:m"), "set_path_max_distance", "get_path_max_distance"); ADD_PROPERTY(PropertyInfo(Variant::INT, "navigation_layers", PROPERTY_HINT_LAYERS_3D_NAVIGATION), "set_navigation_layers", "get_navigation_layers"); ADD_PROPERTY(PropertyInfo(Variant::INT, "pathfinding_algorithm", PROPERTY_HINT_ENUM, "AStar"), "set_pathfinding_algorithm", "get_pathfinding_algorithm"); @@ -116,12 +140,18 @@ void NavigationAgent3D::_bind_methods() { ADD_GROUP("Avoidance", ""); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "avoidance_enabled"), "set_avoidance_enabled", "get_avoidance_enabled"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.1,100,0.01,or_greater,suffix:m"), "set_radius", "get_radius"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "velocity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_velocity", "get_velocity"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "height", PROPERTY_HINT_RANGE, "0.01,100,0.01,or_greater,suffix:m"), "set_height", "get_height"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.01,100,0.01,or_greater,suffix:m"), "set_radius", "get_radius"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "neighbor_distance", PROPERTY_HINT_RANGE, "0.1,10000,0.01,or_greater,suffix:m"), "set_neighbor_distance", "get_neighbor_distance"); ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1,or_greater"), "set_max_neighbors", "get_max_neighbors"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "time_horizon", PROPERTY_HINT_RANGE, "0.01,10,0.01,or_greater,suffix:s"), "set_time_horizon", "get_time_horizon"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "max_speed", PROPERTY_HINT_RANGE, "0.1,1000,0.01,or_greater,suffix:m/s"), "set_max_speed", "get_max_speed"); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "ignore_y"), "set_ignore_y", "get_ignore_y"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "time_horizon_agents", PROPERTY_HINT_RANGE, "0.0,10,0.01,or_greater,suffix:s"), "set_time_horizon_agents", "get_time_horizon_agents"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "time_horizon_obstacles", PROPERTY_HINT_RANGE, "0.0,10,0.01,or_greater,suffix:s"), "set_time_horizon_obstacles", "get_time_horizon_obstacles"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "max_speed", PROPERTY_HINT_RANGE, "0.01,10000,0.01,or_greater,suffix:m/s"), "set_max_speed", "get_max_speed"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_3d_avoidance"), "set_use_3d_avoidance", "get_use_3d_avoidance"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_mask", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_mask", "get_avoidance_mask"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "avoidance_priority", PROPERTY_HINT_RANGE, "0.0,1.0,0.01"), "set_avoidance_priority", "get_avoidance_priority"); ADD_SIGNAL(MethodInfo("path_changed")); ADD_SIGNAL(MethodInfo("target_reached")); @@ -146,6 +176,42 @@ void NavigationAgent3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "debug_path_custom_point_size", PROPERTY_HINT_RANGE, "0,50,0.01,or_greater,suffix:px"), "set_debug_path_custom_point_size", "get_debug_path_custom_point_size"); } +#ifndef DISABLE_DEPRECATED +// Compatibility with Godot 4.0 beta 10 or below. +// Functions in block below all renamed or replaced in 4.0 beta 1X avoidance rework. +bool NavigationAgent3D::_set(const StringName &p_name, const Variant &p_value) { + if (p_name == "time_horizon") { + set_time_horizon_agents(p_value); + return true; + } + if (p_name == "target_location") { + set_target_position(p_value); + return true; + } + if (p_name == "agent_height_offset") { + set_path_height_offset(p_value); + return true; + } + return false; +} + +bool NavigationAgent3D::_get(const StringName &p_name, Variant &r_ret) const { + if (p_name == "time_horizon") { + r_ret = get_time_horizon_agents(); + return true; + } + if (p_name == "target_location") { + r_ret = get_target_position(); + return true; + } + if (p_name == "agent_height_offset") { + r_ret = get_path_height_offset(); + return true; + } + return false; +} +#endif // DISABLE_DEPRECATED + void NavigationAgent3D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_POST_ENTER_TREE: { @@ -154,11 +220,16 @@ void NavigationAgent3D::_notification(int p_what) { set_agent_parent(get_parent()); set_physics_process_internal(true); + if (avoidance_enabled) { + NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin); + } + #ifdef DEBUG_ENABLED if (NavigationServer3D::get_singleton()->get_debug_enabled()) { debug_path_dirty = true; } #endif // DEBUG_ENABLED + } break; case NOTIFICATION_PARENTED: { @@ -211,10 +282,22 @@ void NavigationAgent3D::_notification(int p_what) { case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { if (agent_parent && target_position_submitted) { - if (avoidance_enabled) { - // agent_position on NavigationServer is avoidance only and has nothing to do with pathfinding - // no point in flooding NavigationServer queue with agent position updates that get send to the void if avoidance is not used - NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position()); + if (velocity_submitted) { + velocity_submitted = false; + if (avoidance_enabled) { + NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin); + if (!use_3d_avoidance) { + stored_y_velocity = velocity.y; + velocity.y = 0.0; + } + NavigationServer3D::get_singleton()->agent_set_velocity(agent, velocity); + } + } + if (velocity_forced_submitted) { + velocity_forced_submitted = false; + if (avoidance_enabled) { + NavigationServer3D::get_singleton()->agent_set_velocity_forced(agent, velocity_forced); + } } _check_distance_to_target(); } @@ -229,12 +312,14 @@ void NavigationAgent3D::_notification(int p_what) { NavigationAgent3D::NavigationAgent3D() { agent = NavigationServer3D::get_singleton()->agent_create(); + NavigationServer3D::get_singleton()->agent_set_neighbor_distance(agent, neighbor_distance); NavigationServer3D::get_singleton()->agent_set_max_neighbors(agent, max_neighbors); - NavigationServer3D::get_singleton()->agent_set_time_horizon(agent, time_horizon); + NavigationServer3D::get_singleton()->agent_set_time_horizon_agents(agent, time_horizon_agents); + NavigationServer3D::get_singleton()->agent_set_time_horizon_obstacles(agent, time_horizon_obstacles); NavigationServer3D::get_singleton()->agent_set_radius(agent, radius); + NavigationServer3D::get_singleton()->agent_set_height(agent, height); NavigationServer3D::get_singleton()->agent_set_max_speed(agent, max_speed); - NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, ignore_y); // Preallocate query and result objects to improve performance. navigation_query = Ref(); @@ -243,6 +328,12 @@ NavigationAgent3D::NavigationAgent3D() { navigation_result = Ref(); navigation_result.instantiate(); + set_avoidance_layers(avoidance_layers); + set_avoidance_mask(avoidance_mask); + set_avoidance_priority(avoidance_priority); + set_use_3d_avoidance(use_3d_avoidance); + set_avoidance_enabled(avoidance_enabled); + #ifdef DEBUG_ENABLED NavigationServer3D::get_singleton()->connect(SNAME("navigation_debug_changed"), callable_mp(this, &NavigationAgent3D::_navigation_debug_changed)); #endif // DEBUG_ENABLED @@ -274,9 +365,11 @@ void NavigationAgent3D::set_avoidance_enabled(bool p_enabled) { avoidance_enabled = p_enabled; if (avoidance_enabled) { - NavigationServer3D::get_singleton()->agent_set_callback(agent, callable_mp(this, &NavigationAgent3D::_avoidance_done)); + NavigationServer3D::get_singleton()->agent_set_avoidance_enabled(agent, true); + NavigationServer3D::get_singleton()->agent_set_avoidance_callback(agent, callable_mp(this, &NavigationAgent3D::_avoidance_done)); } else { - NavigationServer3D::get_singleton()->agent_set_callback(agent, Callable()); + NavigationServer3D::get_singleton()->agent_set_avoidance_enabled(agent, false); + NavigationServer3D::get_singleton()->agent_set_avoidance_callback(agent, Callable()); } } @@ -290,7 +383,7 @@ void NavigationAgent3D::set_agent_parent(Node *p_agent_parent) { } // remove agent from any avoidance map before changing parent or there will be leftovers on the RVO map - NavigationServer3D::get_singleton()->agent_set_callback(agent, Callable()); + NavigationServer3D::get_singleton()->agent_set_avoidance_callback(agent, Callable()); if (Object::cast_to(p_agent_parent) != nullptr) { // place agent on navigation map first or else the RVO agent callback creation fails silently later @@ -303,7 +396,7 @@ void NavigationAgent3D::set_agent_parent(Node *p_agent_parent) { // create new avoidance callback if enabled if (avoidance_enabled) { - NavigationServer3D::get_singleton()->agent_set_callback(agent, callable_mp(this, &NavigationAgent3D::_avoidance_done)); + NavigationServer3D::get_singleton()->agent_set_avoidance_callback(agent, callable_mp(this, &NavigationAgent3D::_avoidance_done)); } } else { agent_parent = nullptr; @@ -408,31 +501,32 @@ void NavigationAgent3D::set_target_desired_distance(real_t p_target_desired_dist } void NavigationAgent3D::set_radius(real_t p_radius) { + ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive."); if (Math::is_equal_approx(radius, p_radius)) { return; } - radius = p_radius; NavigationServer3D::get_singleton()->agent_set_radius(agent, radius); } -void NavigationAgent3D::set_agent_height_offset(real_t p_agent_height_offset) { - if (Math::is_equal_approx(navigation_height_offset, p_agent_height_offset)) { +void NavigationAgent3D::set_height(real_t p_height) { + ERR_FAIL_COND_MSG(p_height < 0.0, "Height must be positive."); + if (Math::is_equal_approx(height, p_height)) { return; } - - navigation_height_offset = p_agent_height_offset; + height = p_height; + NavigationServer3D::get_singleton()->agent_set_height(agent, height); } -void NavigationAgent3D::set_ignore_y(bool p_ignore_y) { - if (ignore_y == p_ignore_y) { - return; - } +void NavigationAgent3D::set_path_height_offset(real_t p_path_height_offset) { + path_height_offset = p_path_height_offset; +} - ignore_y = p_ignore_y; - - NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, ignore_y); +void NavigationAgent3D::set_use_3d_avoidance(bool p_use_3d_avoidance) { + use_3d_avoidance = p_use_3d_avoidance; + NavigationServer3D::get_singleton()->agent_set_use_3d_avoidance(agent, use_3d_avoidance); + notify_property_list_changed(); } void NavigationAgent3D::set_neighbor_distance(real_t p_distance) { @@ -455,21 +549,29 @@ void NavigationAgent3D::set_max_neighbors(int p_count) { NavigationServer3D::get_singleton()->agent_set_max_neighbors(agent, max_neighbors); } -void NavigationAgent3D::set_time_horizon(real_t p_time) { - if (Math::is_equal_approx(time_horizon, p_time)) { +void NavigationAgent3D::set_time_horizon_agents(real_t p_time_horizon) { + ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive."); + if (Math::is_equal_approx(time_horizon_agents, p_time_horizon)) { return; } + time_horizon_agents = p_time_horizon; + NavigationServer3D::get_singleton()->agent_set_time_horizon_agents(agent, time_horizon_agents); +} - time_horizon = p_time; - - NavigationServer3D::get_singleton()->agent_set_time_horizon(agent, time_horizon); +void NavigationAgent3D::set_time_horizon_obstacles(real_t p_time_horizon) { + ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive."); + if (Math::is_equal_approx(time_horizon_obstacles, p_time_horizon)) { + return; + } + time_horizon_obstacles = p_time_horizon; + NavigationServer3D::get_singleton()->agent_set_time_horizon_obstacles(agent, time_horizon_obstacles); } void NavigationAgent3D::set_max_speed(real_t p_max_speed) { + ERR_FAIL_COND_MSG(p_max_speed < 0.0, "Max speed must be positive."); if (Math::is_equal_approx(max_speed, p_max_speed)) { return; } - max_speed = p_max_speed; NavigationServer3D::get_singleton()->agent_set_max_speed(agent, max_speed); @@ -509,7 +611,7 @@ Vector3 NavigationAgent3D::get_next_path_position() { ERR_FAIL_COND_V_MSG(agent_parent == nullptr, Vector3(), "The agent has no parent."); return agent_parent->get_global_position(); } else { - return navigation_path[navigation_path_index] - Vector3(0, navigation_height_offset, 0); + return navigation_path[navigation_path_index] - Vector3(0, path_height_offset, 0); } } @@ -541,28 +643,26 @@ Vector3 NavigationAgent3D::get_final_position() { return navigation_path[navigation_path.size() - 1]; } -void NavigationAgent3D::set_velocity(Vector3 p_velocity) { +void NavigationAgent3D::set_velocity_forced(Vector3 p_velocity) { // Intentionally not checking for equality of the parameter. // We need to always submit the velocity to the navigation server, even when it is the same, in order to run avoidance every frame. // Revisit later when the navigation server can update avoidance without users resubmitting the velocity. - target_velocity = p_velocity; - velocity_submitted = true; + velocity_forced = p_velocity; + velocity_forced_submitted = true; +} - NavigationServer3D::get_singleton()->agent_set_target_velocity(agent, target_velocity); - NavigationServer3D::get_singleton()->agent_set_velocity(agent, prev_safe_velocity); +void NavigationAgent3D::set_velocity(const Vector3 p_velocity) { + velocity = p_velocity; + velocity_submitted = true; } void NavigationAgent3D::_avoidance_done(Vector3 p_new_velocity) { - prev_safe_velocity = p_new_velocity; - - if (!velocity_submitted) { - target_velocity = Vector3(); - return; + safe_velocity = p_new_velocity; + if (!use_3d_avoidance) { + safe_velocity.y = stored_y_velocity; } - velocity_submitted = false; - - emit_signal(SNAME("velocity_computed"), p_new_velocity); + emit_signal(SNAME("velocity_computed"), safe_velocity); } PackedStringArray NavigationAgent3D::get_configuration_warnings() const { @@ -585,9 +685,6 @@ void NavigationAgent3D::update_navigation() { if (!target_position_submitted) { return; } - if (update_frame_id == Engine::get_singleton()->get_physics_frames()) { - return; - } update_frame_id = Engine::get_singleton()->get_physics_frames(); @@ -607,8 +704,8 @@ void NavigationAgent3D::update_navigation() { Vector3 segment[2]; segment[0] = navigation_path[navigation_path_index - 1]; segment[1] = navigation_path[navigation_path_index]; - segment[0].y -= navigation_height_offset; - segment[1].y -= navigation_height_offset; + segment[0].y -= path_height_offset; + segment[1].y -= path_height_offset; Vector3 p = Geometry3D::get_closest_point_to_segment(origin, segment); if (origin.distance_to(p) >= path_max_distance) { // To faraway, reload path @@ -650,7 +747,7 @@ void NavigationAgent3D::update_navigation() { const TypedArray &navigation_path_rids = navigation_result->get_path_rids(); const Vector &navigation_path_owners = navigation_result->get_path_owner_ids(); - while (origin.distance_to(navigation_path[navigation_path_index] - Vector3(0, navigation_height_offset, 0)) < path_desired_distance) { + while (origin.distance_to(navigation_path[navigation_path_index] - Vector3(0, path_height_offset, 0)) < path_desired_distance) { Dictionary details; const Vector3 waypoint = navigation_path[navigation_path_index]; @@ -735,6 +832,71 @@ void NavigationAgent3D::_check_distance_to_target() { } } +void NavigationAgent3D::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + NavigationServer3D::get_singleton()->agent_set_avoidance_layers(get_rid(), avoidance_layers); +} + +uint32_t NavigationAgent3D::get_avoidance_layers() const { + return avoidance_layers; +} + +void NavigationAgent3D::set_avoidance_mask(uint32_t p_mask) { + avoidance_mask = p_mask; + NavigationServer3D::get_singleton()->agent_set_avoidance_mask(get_rid(), avoidance_mask); +} + +uint32_t NavigationAgent3D::get_avoidance_mask() const { + return avoidance_mask; +} + +void NavigationAgent3D::set_avoidance_layer_value(int p_layer_number, bool p_value) { + ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive."); + uint32_t avoidance_layers_new = get_avoidance_layers(); + if (p_value) { + avoidance_layers_new |= 1 << (p_layer_number - 1); + } else { + avoidance_layers_new &= ~(1 << (p_layer_number - 1)); + } + set_avoidance_layers(avoidance_layers_new); +} + +bool NavigationAgent3D::get_avoidance_layer_value(int p_layer_number) const { + ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive."); + return get_avoidance_layers() & (1 << (p_layer_number - 1)); +} + +void NavigationAgent3D::set_avoidance_mask_value(int p_mask_number, bool p_value) { + ERR_FAIL_COND_MSG(p_mask_number < 1, "Avoidance mask number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_mask_number > 32, "Avoidance mask number must be between 1 and 32 inclusive."); + uint32_t mask = get_avoidance_mask(); + if (p_value) { + mask |= 1 << (p_mask_number - 1); + } else { + mask &= ~(1 << (p_mask_number - 1)); + } + set_avoidance_mask(mask); +} + +bool NavigationAgent3D::get_avoidance_mask_value(int p_mask_number) const { + ERR_FAIL_COND_V_MSG(p_mask_number < 1, false, "Avoidance mask number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_mask_number > 32, false, "Avoidance mask number must be between 1 and 32 inclusive."); + return get_avoidance_mask() & (1 << (p_mask_number - 1)); +} + +void NavigationAgent3D::set_avoidance_priority(real_t p_priority) { + ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive."); + avoidance_priority = p_priority; + NavigationServer3D::get_singleton()->agent_set_avoidance_priority(get_rid(), p_priority); +} + +real_t NavigationAgent3D::get_avoidance_priority() const { + return avoidance_priority; +} + ////////DEBUG//////////////////////////////////////////////////////////// void NavigationAgent3D::set_debug_enabled(bool p_enabled) { diff --git a/scene/3d/navigation_agent_3d.h b/scene/3d/navigation_agent_3d.h index 16d702f9a57..b74e816d43a 100644 --- a/scene/3d/navigation_agent_3d.h +++ b/scene/3d/navigation_agent_3d.h @@ -47,6 +47,10 @@ class NavigationAgent3D : public Node { RID map_override; bool avoidance_enabled = false; + bool use_3d_avoidance = false; + uint32_t avoidance_layers = 1; + uint32_t avoidance_mask = 1; + real_t avoidance_priority = 1.0; uint32_t navigation_layers = 1; NavigationPathQueryParameters3D::PathfindingAlgorithm pathfinding_algorithm = NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; NavigationPathQueryParameters3D::PathPostProcessing path_postprocessing = NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; @@ -54,24 +58,41 @@ class NavigationAgent3D : public Node { real_t path_desired_distance = 1.0; real_t target_desired_distance = 1.0; + real_t height = 1.0; real_t radius = 0.5; - real_t navigation_height_offset = 0.0; - bool ignore_y = true; + real_t path_height_offset = 0.0; real_t neighbor_distance = 50.0; int max_neighbors = 10; - real_t time_horizon = 1.0; + real_t time_horizon_agents = 1.0; + real_t time_horizon_obstacles = 0.0; real_t max_speed = 10.0; real_t path_max_distance = 5.0; Vector3 target_position; - bool target_position_submitted = false; + Ref navigation_query; Ref navigation_result; int navigation_path_index = 0; + + // the velocity result of the avoidance simulation step + Vector3 safe_velocity; + + /// The submitted target velocity, sets the "wanted" rvo agent velocity on the next update + // this velocity is not guaranteed, the simulation will try to fulfil it if possible + // if other agents or obstacles interfere it will be changed accordingly + Vector3 velocity; bool velocity_submitted = false; - Vector3 prev_safe_velocity; - /// The submitted target velocity - Vector3 target_velocity; + + /// The submitted forced velocity, overrides the rvo agent velocity on the next update + // should only be used very intentionally and not every frame as it interferes with the simulation stability + Vector3 velocity_forced; + bool velocity_forced_submitted = false; + + // 2D avoidance has no y-axis. This stores and reapplies the y-axis velocity to the agent before and after the avoidance step. + // While not perfect it at least looks way better than agent's that clip through everything that is not a flat surface + float stored_y_velocity = 0.0; + + bool target_position_submitted = false; bool target_reached = false; bool navigation_finished = true; // No initialized on purpose @@ -89,23 +110,22 @@ class NavigationAgent3D : public Node { Ref debug_path_mesh; Ref debug_agent_path_line_custom_material; Ref debug_agent_path_point_custom_material; - -private: - void _navigation_debug_changed(); - void _update_debug_path(); #endif // DEBUG_ENABLED protected: static void _bind_methods(); void _notification(int p_what); +#ifndef DISABLE_DEPRECATED + bool _set(const StringName &p_name, const Variant &p_value); + bool _get(const StringName &p_name, Variant &r_ret) const; +#endif // DISABLE_DEPRECATED + public: NavigationAgent3D(); virtual ~NavigationAgent3D(); - RID get_rid() const { - return agent; - } + RID get_rid() const { return agent; } void set_avoidance_enabled(bool p_enabled); bool get_avoidance_enabled() const; @@ -137,49 +157,37 @@ public: RID get_navigation_map() const; void set_path_desired_distance(real_t p_dd); - real_t get_path_desired_distance() const { - return path_desired_distance; - } + real_t get_path_desired_distance() const { return path_desired_distance; } void set_target_desired_distance(real_t p_dd); - real_t get_target_desired_distance() const { - return target_desired_distance; - } + 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; - } + 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_height(real_t p_height); + real_t get_height() const { return height; } - void set_ignore_y(bool p_ignore_y); - bool get_ignore_y() const { - return ignore_y; - } + void set_path_height_offset(real_t p_path_height_offset); + real_t get_path_height_offset() const { return path_height_offset; } + + void set_use_3d_avoidance(bool p_use_3d_avoidance); + bool get_use_3d_avoidance() const { return use_3d_avoidance; } void set_neighbor_distance(real_t p_distance); - real_t get_neighbor_distance() const { - return neighbor_distance; - } + real_t get_neighbor_distance() const { return neighbor_distance; } void set_max_neighbors(int p_count); - int get_max_neighbors() const { - return max_neighbors; - } + 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_time_horizon_agents(real_t p_time_horizon); + real_t get_time_horizon_agents() const { return time_horizon_agents; } + + void set_time_horizon_obstacles(real_t p_time_horizon); + real_t get_time_horizon_obstacles() const { return time_horizon_obstacles; } void set_max_speed(real_t p_max_speed); - real_t get_max_speed() const { - return 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(); @@ -189,15 +197,11 @@ public: Vector3 get_next_path_position(); - Ref get_current_navigation_result() const { - return navigation_result; - } - const Vector &get_current_navigation_path() const { - return navigation_result->get_path(); - } - int get_current_navigation_path_index() const { - return navigation_path_index; - } + Ref get_current_navigation_result() const { return navigation_result; } + + const Vector &get_current_navigation_path() const { return navigation_result->get_path(); } + + int get_current_navigation_path_index() const { return navigation_path_index; } real_t distance_to_target() const; bool is_target_reached() const; @@ -205,11 +209,30 @@ public: bool is_navigation_finished(); Vector3 get_final_position(); - void set_velocity(Vector3 p_velocity); + void set_velocity(const Vector3 p_velocity); + Vector3 get_velocity() { return velocity; } + + void set_velocity_forced(const Vector3 p_velocity); + void _avoidance_done(Vector3 p_new_velocity); PackedStringArray get_configuration_warnings() const override; + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const; + + void set_avoidance_mask(uint32_t p_mask); + uint32_t get_avoidance_mask() const; + + void set_avoidance_layer_value(int p_layer_number, bool p_value); + bool get_avoidance_layer_value(int p_layer_number) const; + + void set_avoidance_mask_value(int p_mask_number, bool p_value); + bool get_avoidance_mask_value(int p_mask_number) const; + + void set_avoidance_priority(real_t p_priority); + real_t get_avoidance_priority() const; + void set_debug_enabled(bool p_enabled); bool get_debug_enabled() const; @@ -226,6 +249,11 @@ private: void update_navigation(); void _request_repath(); void _check_distance_to_target(); + +#ifdef DEBUG_ENABLED + void _navigation_debug_changed(); + void _update_debug_path(); +#endif // DEBUG_ENABLED }; #endif // NAVIGATION_AGENT_3D_H diff --git a/scene/3d/navigation_link_3d.cpp b/scene/3d/navigation_link_3d.cpp index 9c4b8e79051..536b0eae5d5 100644 --- a/scene/3d/navigation_link_3d.cpp +++ b/scene/3d/navigation_link_3d.cpp @@ -45,7 +45,7 @@ void NavigationLink3D::_update_debug_mesh() { return; } - if (!NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (!NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { if (debug_instance.is_valid()) { RS::get_singleton()->instance_set_visible(debug_instance, false); } diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp index 14d93fb0e03..b44ebf837ac 100644 --- a/scene/3d/navigation_obstacle_3d.cpp +++ b/scene/3d/navigation_obstacle_3d.cpp @@ -30,230 +30,524 @@ #include "navigation_obstacle_3d.h" +#include "core/math/geometry_2d.h" #include "scene/3d/collision_shape_3d.h" #include "scene/3d/physics_body_3d.h" #include "servers/navigation_server_3d.h" void NavigationObstacle3D::_bind_methods() { - ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle3D::get_rid); + ClassDB::bind_method(D_METHOD("get_obstacle_rid"), &NavigationObstacle3D::get_obstacle_rid); + ClassDB::bind_method(D_METHOD("get_agent_rid"), &NavigationObstacle3D::get_agent_rid); ClassDB::bind_method(D_METHOD("set_navigation_map", "navigation_map"), &NavigationObstacle3D::set_navigation_map); ClassDB::bind_method(D_METHOD("get_navigation_map"), &NavigationObstacle3D::get_navigation_map); - ClassDB::bind_method(D_METHOD("set_estimate_radius", "estimate_radius"), &NavigationObstacle3D::set_estimate_radius); - ClassDB::bind_method(D_METHOD("is_radius_estimated"), &NavigationObstacle3D::is_radius_estimated); ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationObstacle3D::set_radius); ClassDB::bind_method(D_METHOD("get_radius"), &NavigationObstacle3D::get_radius); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "estimate_radius"), "set_estimate_radius", "is_radius_estimated"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.01,100,0.01"), "set_radius", "get_radius"); -} + ClassDB::bind_method(D_METHOD("set_height", "height"), &NavigationObstacle3D::set_height); + ClassDB::bind_method(D_METHOD("get_height"), &NavigationObstacle3D::get_height); -void NavigationObstacle3D::_validate_property(PropertyInfo &p_property) const { - if (p_property.name == "radius") { - if (estimate_radius) { - p_property.usage = PROPERTY_USAGE_NO_EDITOR; - } - } + ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationObstacle3D::set_velocity); + ClassDB::bind_method(D_METHOD("get_velocity"), &NavigationObstacle3D::get_velocity); + + ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationObstacle3D::set_vertices); + ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationObstacle3D::get_vertices); + + ClassDB::bind_method(D_METHOD("set_avoidance_layers", "layers"), &NavigationObstacle3D::set_avoidance_layers); + ClassDB::bind_method(D_METHOD("get_avoidance_layers"), &NavigationObstacle3D::get_avoidance_layers); + + ClassDB::bind_method(D_METHOD("set_avoidance_layer_value", "layer_number", "value"), &NavigationObstacle3D::set_avoidance_layer_value); + ClassDB::bind_method(D_METHOD("get_avoidance_layer_value", "layer_number"), &NavigationObstacle3D::get_avoidance_layer_value); + + ClassDB::bind_method(D_METHOD("set_use_3d_avoidance", "enabled"), &NavigationObstacle3D::set_use_3d_avoidance); + ClassDB::bind_method(D_METHOD("get_use_3d_avoidance"), &NavigationObstacle3D::get_use_3d_avoidance); + + ADD_GROUP("Avoidance", "avoidance_"); + ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "velocity", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR), "set_velocity", "get_velocity"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.0,100,0.01,suffix:m"), "set_radius", "get_radius"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "height", PROPERTY_HINT_RANGE, "0.0,100,0.01,suffix:m"), "set_height", "get_height"); + ADD_PROPERTY(PropertyInfo(Variant::PACKED_VECTOR3_ARRAY, "vertices"), "set_vertices", "get_vertices"); + ADD_PROPERTY(PropertyInfo(Variant::INT, "avoidance_layers", PROPERTY_HINT_LAYERS_AVOIDANCE), "set_avoidance_layers", "get_avoidance_layers"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_3d_avoidance"), "set_use_3d_avoidance", "get_use_3d_avoidance"); } void NavigationObstacle3D::_notification(int p_what) { switch (p_what) { case NOTIFICATION_POST_ENTER_TREE: { - set_agent_parent(get_parent()); + if (map_override.is_valid()) { + _update_map(map_override); + } else if (is_inside_tree()) { + _update_map(get_world_3d()->get_navigation_map()); + } else { + _update_map(RID()); + } + previous_transform = get_global_transform(); + // need to trigger map controlled agent assignment somehow for the fake_agent since obstacles use no callback like regular agents + NavigationServer3D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0); + _update_position(get_global_transform().origin); set_physics_process_internal(true); +#ifdef DEBUG_ENABLED + if ((NavigationServer3D::get_singleton()->get_debug_avoidance_enabled()) && + (NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius())) { + _update_fake_agent_radius_debug(); + _update_static_obstacle_debug(); + } +#endif // DEBUG_ENABLED } break; case NOTIFICATION_EXIT_TREE: { - set_agent_parent(nullptr); set_physics_process_internal(false); - } break; - - case NOTIFICATION_PARENTED: { - if (is_inside_tree() && (get_parent() != parent_node3d)) { - set_agent_parent(get_parent()); - set_physics_process_internal(true); + _update_map(RID()); +#ifdef DEBUG_ENABLED + if (fake_agent_radius_debug_instance.is_valid()) { + RS::get_singleton()->instance_set_visible(fake_agent_radius_debug_instance, false); } - } break; - - case NOTIFICATION_UNPARENTED: { - set_agent_parent(nullptr); - set_physics_process_internal(false); + if (static_obstacle_debug_instance.is_valid()) { + RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false); + } +#endif // DEBUG_ENABLED } break; case NOTIFICATION_PAUSED: { - if (parent_node3d && !parent_node3d->can_process()) { - map_before_pause = NavigationServer3D::get_singleton()->agent_get_map(get_rid()); - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID()); - } else if (parent_node3d && parent_node3d->can_process() && !(map_before_pause == RID())) { - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_before_pause); + if (!can_process()) { + map_before_pause = map_current; + _update_map(RID()); + } else if (can_process() && !(map_before_pause == RID())) { + _update_map(map_before_pause); map_before_pause = RID(); } } break; case NOTIFICATION_UNPAUSED: { - if (parent_node3d && !parent_node3d->can_process()) { - map_before_pause = NavigationServer3D::get_singleton()->agent_get_map(get_rid()); - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID()); - } else if (parent_node3d && parent_node3d->can_process() && !(map_before_pause == RID())) { - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_before_pause); + if (!can_process()) { + map_before_pause = map_current; + _update_map(RID()); + } else if (can_process() && !(map_before_pause == RID())) { + _update_map(map_before_pause); map_before_pause = RID(); } } break; case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: { - if (parent_node3d && parent_node3d->is_inside_tree()) { - NavigationServer3D::get_singleton()->agent_set_position(agent, parent_node3d->get_global_transform().origin); + if (is_inside_tree()) { + _update_position(get_global_transform().origin); - PhysicsBody3D *rigid = Object::cast_to(get_parent()); - if (rigid) { - Vector3 v = rigid->get_linear_velocity(); - NavigationServer3D::get_singleton()->agent_set_velocity(agent, v); - NavigationServer3D::get_singleton()->agent_set_target_velocity(agent, v); + if (velocity_submitted) { + velocity_submitted = false; + // only update if there is a noticeable change, else the rvo agent preferred velocity stays the same + if (!previous_velocity.is_equal_approx(velocity)) { + NavigationServer3D::get_singleton()->agent_set_velocity(fake_agent, velocity); + } + previous_velocity = velocity; } +#ifdef DEBUG_ENABLED + if (fake_agent_radius_debug_instance.is_valid() && radius > 0.0) { + RS::get_singleton()->instance_set_transform(fake_agent_radius_debug_instance, get_global_transform()); + } + if (static_obstacle_debug_instance.is_valid() && get_vertices().size() > 0) { + RS::get_singleton()->instance_set_transform(static_obstacle_debug_instance, get_global_transform()); + } +#endif // DEBUG_ENABLED } } break; } } NavigationObstacle3D::NavigationObstacle3D() { - agent = NavigationServer3D::get_singleton()->agent_create(); - initialize_agent(); + obstacle = NavigationServer3D::get_singleton()->obstacle_create(); + fake_agent = NavigationServer3D::get_singleton()->agent_create(); + + // change properties of the fake agent so it can act as fake obstacle with a radius + NavigationServer3D::get_singleton()->agent_set_neighbor_distance(fake_agent, 0.0); + NavigationServer3D::get_singleton()->agent_set_max_neighbors(fake_agent, 0); + NavigationServer3D::get_singleton()->agent_set_time_horizon_agents(fake_agent, 0.0); + NavigationServer3D::get_singleton()->agent_set_time_horizon_obstacles(fake_agent, 0.0); + NavigationServer3D::get_singleton()->agent_set_max_speed(fake_agent, 0.0); + NavigationServer3D::get_singleton()->agent_set_avoidance_mask(fake_agent, 1); + NavigationServer3D::get_singleton()->agent_set_avoidance_priority(fake_agent, 1.0); + NavigationServer3D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0); + + set_radius(radius); + set_height(height); + set_vertices(vertices); + set_avoidance_layers(avoidance_layers); + set_use_3d_avoidance(use_3d_avoidance); + +#ifdef DEBUG_ENABLED + NavigationServer3D::get_singleton()->connect("avoidance_debug_changed", callable_mp(this, &NavigationObstacle3D::_update_fake_agent_radius_debug)); + NavigationServer3D::get_singleton()->connect("avoidance_debug_changed", callable_mp(this, &NavigationObstacle3D::_update_static_obstacle_debug)); + _update_fake_agent_radius_debug(); + _update_static_obstacle_debug(); +#endif // DEBUG_ENABLED } NavigationObstacle3D::~NavigationObstacle3D() { ERR_FAIL_NULL(NavigationServer3D::get_singleton()); - NavigationServer3D::get_singleton()->free(agent); - agent = RID(); // Pointless -} -PackedStringArray NavigationObstacle3D::get_configuration_warnings() const { - PackedStringArray warnings = Node::get_configuration_warnings(); + NavigationServer3D::get_singleton()->free(obstacle); + obstacle = RID(); - if (!Object::cast_to(get_parent())) { - warnings.push_back(RTR("The NavigationObstacle3D only serves to provide collision avoidance to a Node3D inheriting parent object.")); + NavigationServer3D::get_singleton()->free(fake_agent); + fake_agent = RID(); + +#ifdef DEBUG_ENABLED + NavigationServer3D::get_singleton()->disconnect("avoidance_debug_changed", callable_mp(this, &NavigationObstacle3D::_update_fake_agent_radius_debug)); + NavigationServer3D::get_singleton()->disconnect("avoidance_debug_changed", callable_mp(this, &NavigationObstacle3D::_update_static_obstacle_debug)); + if (fake_agent_radius_debug_instance.is_valid()) { + RenderingServer::get_singleton()->free(fake_agent_radius_debug_instance); + } + if (fake_agent_radius_debug_mesh.is_valid()) { + RenderingServer::get_singleton()->free(fake_agent_radius_debug_mesh->get_rid()); } - if (Object::cast_to(get_parent())) { - warnings.push_back(RTR("The NavigationObstacle3D is intended for constantly moving bodies like CharacterBody3D or RigidBody3D as it creates only an RVO avoidance radius and does not follow scene geometry exactly." - "\nNot constantly moving or complete static objects should be (re)baked to a NavigationMesh so agents can not only avoid them but also move along those objects outline at high detail")); + if (static_obstacle_debug_instance.is_valid()) { + RenderingServer::get_singleton()->free(static_obstacle_debug_instance); } - - return warnings; + if (static_obstacle_debug_mesh.is_valid()) { + RenderingServer::get_singleton()->free(static_obstacle_debug_mesh->get_rid()); + } +#endif // DEBUG_ENABLED } -void NavigationObstacle3D::initialize_agent() { - NavigationServer3D::get_singleton()->agent_set_neighbor_distance(agent, 0.0); - NavigationServer3D::get_singleton()->agent_set_max_neighbors(agent, 0); - NavigationServer3D::get_singleton()->agent_set_time_horizon(agent, 0.0); - NavigationServer3D::get_singleton()->agent_set_max_speed(agent, 0.0); -} - -void NavigationObstacle3D::reevaluate_agent_radius() { - if (!estimate_radius) { - NavigationServer3D::get_singleton()->agent_set_radius(agent, radius); - } else if (parent_node3d && parent_node3d->is_inside_tree()) { - NavigationServer3D::get_singleton()->agent_set_radius(agent, estimate_agent_radius()); - } -} - -real_t NavigationObstacle3D::estimate_agent_radius() const { - if (parent_node3d && parent_node3d->is_inside_tree()) { - // Estimate the radius of this physics body - real_t max_radius = 0.0; - for (int i(0); i < parent_node3d->get_child_count(); i++) { - // For each collision shape - CollisionShape3D *cs = Object::cast_to(parent_node3d->get_child(i)); - if (cs && cs->is_inside_tree()) { - // Take the distance between the Body center to the shape center - real_t r = cs->get_transform().origin.length(); - if (cs->get_shape().is_valid()) { - // and add the enclosing shape radius - r += cs->get_shape()->get_enclosing_radius(); - } - Vector3 s = cs->get_global_transform().basis.get_scale(); - r *= MAX(s.x, MAX(s.y, s.z)); - // Takes the biggest radius - max_radius = MAX(max_radius, r); - } else if (cs && !cs->is_inside_tree()) { - WARN_PRINT("A CollisionShape3D of the NavigationObstacle3D parent node was not inside the SceneTree when estimating the obstacle radius." - "\nMove the NavigationObstacle3D to a child position below any CollisionShape3D node of the parent node so the CollisionShape3D is already inside the SceneTree."); - } - } - - Vector3 s = parent_node3d->get_global_transform().basis.get_scale(); - max_radius *= MAX(s.x, MAX(s.y, s.z)); - - if (max_radius > 0.0) { - return max_radius; - } - } - return 1.0; // Never a 0 radius -} - -void NavigationObstacle3D::set_agent_parent(Node *p_agent_parent) { - if (parent_node3d == p_agent_parent) { - return; - } - - if (Object::cast_to(p_agent_parent) != nullptr) { - parent_node3d = Object::cast_to(p_agent_parent); - if (map_override.is_valid()) { - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_override); - } else { - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), parent_node3d->get_world_3d()->get_navigation_map()); - } - // Need to register Callback as obstacle requires a valid Callback to be added to avoidance simulation. - NavigationServer3D::get_singleton()->agent_set_callback(get_rid(), callable_mp(this, &NavigationObstacle3D::_avoidance_done)); - reevaluate_agent_radius(); - } else { - parent_node3d = nullptr; - NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID()); - NavigationServer3D::get_singleton()->agent_set_callback(agent, Callable()); - } -} - -void NavigationObstacle3D::_avoidance_done(Vector3 p_new_velocity) { - // Dummy function as obstacle requires a valid Callback to be added to avoidance simulation. +void NavigationObstacle3D::set_vertices(const Vector &p_vertices) { + vertices = p_vertices; + NavigationServer3D::get_singleton()->obstacle_set_vertices(obstacle, vertices); +#ifdef DEBUG_ENABLED + _update_static_obstacle_debug(); +#endif // DEBUG_ENABLED } void NavigationObstacle3D::set_navigation_map(RID p_navigation_map) { if (map_override == p_navigation_map) { return; } - map_override = p_navigation_map; - - NavigationServer3D::get_singleton()->agent_set_map(agent, map_override); + _update_map(map_override); } RID NavigationObstacle3D::get_navigation_map() const { if (map_override.is_valid()) { return map_override; - } else if (parent_node3d != nullptr) { - return parent_node3d->get_world_3d()->get_navigation_map(); + } else if (is_inside_tree()) { + return get_world_3d()->get_navigation_map(); } return RID(); } -void NavigationObstacle3D::set_estimate_radius(bool p_estimate_radius) { - if (estimate_radius == p_estimate_radius) { - return; - } - - estimate_radius = p_estimate_radius; - - notify_property_list_changed(); - reevaluate_agent_radius(); -} - void NavigationObstacle3D::set_radius(real_t p_radius) { - ERR_FAIL_COND_MSG(p_radius <= 0.0, "Radius must be greater than 0."); + ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive."); if (Math::is_equal_approx(radius, p_radius)) { return; } radius = p_radius; + NavigationServer3D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0.0); + NavigationServer3D::get_singleton()->agent_set_radius(fake_agent, radius); - reevaluate_agent_radius(); +#ifdef DEBUG_ENABLED + _update_fake_agent_radius_debug(); +#endif // DEBUG_ENABLED } + +void NavigationObstacle3D::set_height(real_t p_height) { + ERR_FAIL_COND_MSG(p_height < 0.0, "Height must be positive."); + if (Math::is_equal_approx(height, p_height)) { + return; + } + + height = p_height; + NavigationServer3D::get_singleton()->obstacle_set_height(obstacle, height); + NavigationServer3D::get_singleton()->agent_set_height(fake_agent, height); +#ifdef DEBUG_ENABLED + _update_static_obstacle_debug(); +#endif // DEBUG_ENABLED +} + +void NavigationObstacle3D::set_avoidance_layers(uint32_t p_layers) { + avoidance_layers = p_layers; + NavigationServer3D::get_singleton()->obstacle_set_avoidance_layers(obstacle, avoidance_layers); + NavigationServer3D::get_singleton()->agent_set_avoidance_layers(fake_agent, avoidance_layers); +} + +uint32_t NavigationObstacle3D::get_avoidance_layers() const { + return avoidance_layers; +} + +void NavigationObstacle3D::set_avoidance_layer_value(int p_layer_number, bool p_value) { + ERR_FAIL_COND_MSG(p_layer_number < 1, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_MSG(p_layer_number > 32, "Avoidance layer number must be between 1 and 32 inclusive."); + uint32_t avoidance_layers_new = get_avoidance_layers(); + if (p_value) { + avoidance_layers_new |= 1 << (p_layer_number - 1); + } else { + avoidance_layers_new &= ~(1 << (p_layer_number - 1)); + } + set_avoidance_layers(avoidance_layers_new); +} + +bool NavigationObstacle3D::get_avoidance_layer_value(int p_layer_number) const { + ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Avoidance layer number must be between 1 and 32 inclusive."); + ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Avoidance layer number must be between 1 and 32 inclusive."); + return get_avoidance_layers() & (1 << (p_layer_number - 1)); +} + +void NavigationObstacle3D::set_use_3d_avoidance(bool p_use_3d_avoidance) { + use_3d_avoidance = p_use_3d_avoidance; + _update_use_3d_avoidance(use_3d_avoidance); + notify_property_list_changed(); +} + +void NavigationObstacle3D::set_velocity(const Vector3 p_velocity) { + velocity = p_velocity; + velocity_submitted = true; +} + +void NavigationObstacle3D::_update_map(RID p_map) { + // avoidance obstacles are 2D only, no point keeping them on the map while 3D is used + if (use_3d_avoidance) { + NavigationServer3D::get_singleton()->obstacle_set_map(obstacle, RID()); + } else { + NavigationServer3D::get_singleton()->obstacle_set_map(obstacle, p_map); + } + NavigationServer3D::get_singleton()->agent_set_map(fake_agent, p_map); + map_current = p_map; +} + +void NavigationObstacle3D::_update_position(const Vector3 p_position) { + if (vertices.size() > 0) { + NavigationServer3D::get_singleton()->obstacle_set_position(obstacle, p_position); + } + if (radius > 0.0) { + NavigationServer3D::get_singleton()->agent_set_position(fake_agent, p_position); + } +} + +void NavigationObstacle3D::_update_use_3d_avoidance(bool p_use_3d_avoidance) { + NavigationServer3D::get_singleton()->agent_set_use_3d_avoidance(fake_agent, use_3d_avoidance); + _update_map(map_current); +} + +#ifdef DEBUG_ENABLED +void NavigationObstacle3D::_update_fake_agent_radius_debug() { + bool is_debug_enabled = false; + if (Engine::get_singleton()->is_editor_hint()) { + is_debug_enabled = true; + } else if (NavigationServer3D::get_singleton()->get_debug_enabled() && + NavigationServer3D::get_singleton()->get_debug_avoidance_enabled() && + NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius()) { + is_debug_enabled = true; + } + + if (is_debug_enabled == false) { + if (fake_agent_radius_debug_instance.is_valid()) { + RS::get_singleton()->instance_set_visible(fake_agent_radius_debug_instance, false); + } + return; + } + + if (!fake_agent_radius_debug_instance.is_valid()) { + fake_agent_radius_debug_instance = RenderingServer::get_singleton()->instance_create(); + } + if (!fake_agent_radius_debug_mesh.is_valid()) { + fake_agent_radius_debug_mesh = Ref(memnew(ArrayMesh)); + } + fake_agent_radius_debug_mesh->clear_surfaces(); + + Vector face_vertex_array; + Vector face_indices_array; + + int i, j, prevrow, thisrow, point; + float x, y, z; + + int rings = 16; + int radial_segments = 32; + + point = 0; + + thisrow = 0; + prevrow = 0; + for (j = 0; j <= (rings + 1); j++) { + float v = j; + float w; + + v /= (rings + 1); + w = sin(Math_PI * v); + y = (radius)*cos(Math_PI * v); + + for (i = 0; i <= radial_segments; i++) { + float u = i; + u /= radial_segments; + + x = sin(u * Math_TAU); + z = cos(u * Math_TAU); + + Vector3 p = Vector3(x * radius * w, y, z * radius * w); + face_vertex_array.push_back(p); + + point++; + + if (i > 0 && j > 0) { + face_indices_array.push_back(prevrow + i - 1); + face_indices_array.push_back(prevrow + i); + face_indices_array.push_back(thisrow + i - 1); + + face_indices_array.push_back(prevrow + i); + face_indices_array.push_back(thisrow + i); + face_indices_array.push_back(thisrow + i - 1); + }; + }; + + prevrow = thisrow; + thisrow = point; + }; + + Array face_mesh_array; + face_mesh_array.resize(Mesh::ARRAY_MAX); + face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array; + face_mesh_array[Mesh::ARRAY_INDEX] = face_indices_array; + + fake_agent_radius_debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array); + Ref face_material = NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_obstacles_radius_material(); + fake_agent_radius_debug_mesh->surface_set_material(0, face_material); + + RS::get_singleton()->instance_set_base(fake_agent_radius_debug_instance, fake_agent_radius_debug_mesh->get_rid()); + if (is_inside_tree()) { + RS::get_singleton()->instance_set_scenario(fake_agent_radius_debug_instance, get_world_3d()->get_scenario()); + RS::get_singleton()->instance_set_visible(fake_agent_radius_debug_instance, is_visible_in_tree()); + } +} +#endif // DEBUG_ENABLED + +#ifdef DEBUG_ENABLED +void NavigationObstacle3D::_update_static_obstacle_debug() { + bool is_debug_enabled = false; + if (Engine::get_singleton()->is_editor_hint()) { + is_debug_enabled = true; + } else if (NavigationServer3D::get_singleton()->get_debug_enabled() && + NavigationServer3D::get_singleton()->get_debug_avoidance_enabled() && + NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) { + is_debug_enabled = true; + } + + if (is_debug_enabled == false) { + if (static_obstacle_debug_instance.is_valid()) { + RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false); + } + return; + } + + if (vertices.size() < 3) { + if (static_obstacle_debug_instance.is_valid()) { + RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false); + } + return; + } + + if (!static_obstacle_debug_instance.is_valid()) { + static_obstacle_debug_instance = RenderingServer::get_singleton()->instance_create(); + } + if (!static_obstacle_debug_mesh.is_valid()) { + static_obstacle_debug_mesh = Ref(memnew(ArrayMesh)); + } + static_obstacle_debug_mesh->clear_surfaces(); + + Vector polygon_2d_vertices; + polygon_2d_vertices.resize(vertices.size()); + Vector2 *polygon_2d_vertices_ptr = polygon_2d_vertices.ptrw(); + + for (int i = 0; i < vertices.size(); ++i) { + Vector3 obstacle_vertex = vertices[i]; + Vector2 obstacle_vertex_2d = Vector2(obstacle_vertex.x, obstacle_vertex.z); + polygon_2d_vertices_ptr[i] = obstacle_vertex_2d; + } + + Vector triangulated_polygon_2d_indices = Geometry2D::triangulate_polygon(polygon_2d_vertices); + + if (triangulated_polygon_2d_indices.is_empty()) { + // failed triangulation + return; + } + + bool obstacle_pushes_inward = Geometry2D::is_polygon_clockwise(polygon_2d_vertices); + + Vector face_vertex_array; + Vector face_indices_array; + + face_vertex_array.resize(polygon_2d_vertices.size()); + face_indices_array.resize(triangulated_polygon_2d_indices.size()); + + Vector3 *face_vertex_array_ptr = face_vertex_array.ptrw(); + int *face_indices_array_ptr = face_indices_array.ptrw(); + + for (int i = 0; i < triangulated_polygon_2d_indices.size(); ++i) { + int vertex_index = triangulated_polygon_2d_indices[i]; + const Vector2 &vertex_2d = polygon_2d_vertices[vertex_index]; + Vector3 vertex_3d = Vector3(vertex_2d.x, 0.0, vertex_2d.y); + face_vertex_array_ptr[vertex_index] = vertex_3d; + face_indices_array_ptr[i] = vertex_index; + } + + Array face_mesh_array; + face_mesh_array.resize(Mesh::ARRAY_MAX); + face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array; + face_mesh_array[Mesh::ARRAY_INDEX] = face_indices_array; + + static_obstacle_debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array); + + Vector edge_vertex_array; + + for (int i = 0; i < polygon_2d_vertices.size(); ++i) { + int from_index = i - 1; + int to_index = i; + + if (i == 0) { + from_index = polygon_2d_vertices.size() - 1; + } + + const Vector2 &vertex_2d_from = polygon_2d_vertices[from_index]; + const Vector2 &vertex_2d_to = polygon_2d_vertices[to_index]; + + Vector3 vertex_3d_ground_from = Vector3(vertex_2d_from.x, 0.0, vertex_2d_from.y); + Vector3 vertex_3d_ground_to = Vector3(vertex_2d_to.x, 0.0, vertex_2d_to.y); + + edge_vertex_array.push_back(vertex_3d_ground_from); + edge_vertex_array.push_back(vertex_3d_ground_to); + + Vector3 vertex_3d_height_from = Vector3(vertex_2d_from.x, height, vertex_2d_from.y); + Vector3 vertex_3d_height_to = Vector3(vertex_2d_to.x, height, vertex_2d_to.y); + + edge_vertex_array.push_back(vertex_3d_height_from); + edge_vertex_array.push_back(vertex_3d_height_to); + + edge_vertex_array.push_back(vertex_3d_ground_from); + edge_vertex_array.push_back(vertex_3d_height_from); + } + + Array edge_mesh_array; + edge_mesh_array.resize(Mesh::ARRAY_MAX); + edge_mesh_array[Mesh::ARRAY_VERTEX] = edge_vertex_array; + + static_obstacle_debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, edge_mesh_array); + + Ref face_material; + Ref edge_material; + + if (obstacle_pushes_inward) { + face_material = NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_material(); + edge_material = NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material(); + } else { + face_material = NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_material(); + edge_material = NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_material(); + } + + static_obstacle_debug_mesh->surface_set_material(0, face_material); + static_obstacle_debug_mesh->surface_set_material(1, edge_material); + + RS::get_singleton()->instance_set_base(static_obstacle_debug_instance, static_obstacle_debug_mesh->get_rid()); + if (is_inside_tree()) { + RS::get_singleton()->instance_set_scenario(static_obstacle_debug_instance, get_world_3d()->get_scenario()); + RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, is_visible_in_tree()); + } +} +#endif // DEBUG_ENABLED diff --git a/scene/3d/navigation_obstacle_3d.h b/scene/3d/navigation_obstacle_3d.h index c5a9d737f6c..603d22cf5ea 100644 --- a/scene/3d/navigation_obstacle_3d.h +++ b/scene/3d/navigation_obstacle_3d.h @@ -33,53 +33,83 @@ #include "scene/3d/node_3d.h" -class NavigationObstacle3D : public Node { - GDCLASS(NavigationObstacle3D, Node); +class NavigationObstacle3D : public Node3D { + GDCLASS(NavigationObstacle3D, Node3D); - Node3D *parent_node3d = nullptr; - - RID agent; + RID obstacle; RID map_before_pause; RID map_override; + RID map_current; - bool estimate_radius = true; - real_t radius = 1.0; + real_t height = 1.0; + real_t radius = 0.0; + + Vector vertices; + + RID fake_agent; + uint32_t avoidance_layers = 1; + + bool use_3d_avoidance = false; + + Transform3D previous_transform; + + Vector3 velocity; + Vector3 previous_velocity; + bool velocity_submitted = false; + +#ifdef DEBUG_ENABLED + RID fake_agent_radius_debug_instance; + Ref fake_agent_radius_debug_mesh; + + RID static_obstacle_debug_instance; + Ref static_obstacle_debug_mesh; + +private: + void _update_fake_agent_radius_debug(); + void _update_static_obstacle_debug(); +#endif // DEBUG_ENABLED protected: static void _bind_methods(); - void _validate_property(PropertyInfo &p_property) const; void _notification(int p_what); public: NavigationObstacle3D(); virtual ~NavigationObstacle3D(); - RID get_rid() const { - return agent; - } - - void set_agent_parent(Node *p_agent_parent); + RID get_obstacle_rid() const { return obstacle; } + RID get_agent_rid() const { return fake_agent; } void set_navigation_map(RID p_navigation_map); RID get_navigation_map() const; - void set_estimate_radius(bool p_estimate_radius); - bool is_radius_estimated() const { - return estimate_radius; - } void set_radius(real_t p_radius); - real_t get_radius() const { - return radius; - } + real_t get_radius() const { return radius; } - PackedStringArray get_configuration_warnings() const override; + void set_height(real_t p_height); + real_t get_height() const { return height; } + + void set_vertices(const Vector &p_vertices); + const Vector &get_vertices() const { return vertices; }; + + void set_avoidance_layers(uint32_t p_layers); + uint32_t get_avoidance_layers() const; + + void set_avoidance_layer_value(int p_layer_number, bool p_value); + bool get_avoidance_layer_value(int p_layer_number) const; + + void set_use_3d_avoidance(bool p_use_3d_avoidance); + bool get_use_3d_avoidance() const { return use_3d_avoidance; } + + void set_velocity(const Vector3 p_velocity); + Vector3 get_velocity() const { return velocity; }; void _avoidance_done(Vector3 p_new_velocity); // Dummy private: - void initialize_agent(); - void reevaluate_agent_radius(); - real_t estimate_agent_radius() const; + void _update_map(RID p_map); + void _update_position(const Vector3 p_position); + void _update_use_3d_avoidance(bool p_use_3d_avoidance); }; #endif // NAVIGATION_OBSTACLE_3D_H diff --git a/scene/3d/navigation_region_3d.cpp b/scene/3d/navigation_region_3d.cpp index 85633c1dc71..cfe0e673a3c 100644 --- a/scene/3d/navigation_region_3d.cpp +++ b/scene/3d/navigation_region_3d.cpp @@ -159,7 +159,7 @@ void NavigationRegion3D::_notification(int p_what) { } #ifdef DEBUG_ENABLED - if (NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { _update_debug_mesh(); } #endif // DEBUG_ENABLED @@ -210,7 +210,7 @@ void NavigationRegion3D::set_navigation_mesh(const Ref &p_naviga NavigationServer3D::get_singleton()->region_set_navigation_mesh(region, p_navigation_mesh); #ifdef DEBUG_ENABLED - if (is_inside_tree() && NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (is_inside_tree() && NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { if (navigation_mesh.is_valid()) { _update_debug_mesh(); _update_debug_edge_connections_mesh(); @@ -413,7 +413,7 @@ void NavigationRegion3D::_update_debug_mesh() { return; } - if (!NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (!NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { if (debug_instance.is_valid()) { RS::get_singleton()->instance_set_visible(debug_instance, false); } @@ -578,7 +578,7 @@ void NavigationRegion3D::_update_debug_mesh() { #ifdef DEBUG_ENABLED void NavigationRegion3D::_update_debug_edge_connections_mesh() { - if (!NavigationServer3D::get_singleton()->get_debug_enabled()) { + if (!NavigationServer3D::get_singleton()->get_debug_navigation_enabled()) { if (debug_edge_connections_instance.is_valid()) { RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, false); } diff --git a/scene/main/scene_tree.cpp b/scene/main/scene_tree.cpp index b6b694ff550..e2008b5a744 100644 --- a/scene/main/scene_tree.cpp +++ b/scene/main/scene_tree.cpp @@ -695,6 +695,14 @@ void SceneTree::set_debug_navigation_hint(bool p_enabled) { bool SceneTree::is_debugging_navigation_hint() const { return debug_navigation_hint; } + +void SceneTree::set_debug_avoidance_hint(bool p_enabled) { + debug_avoidance_hint = p_enabled; +} + +bool SceneTree::is_debugging_avoidance_hint() const { + return debug_avoidance_hint; +} #endif void SceneTree::set_debug_collisions_color(const Color &p_color) { diff --git a/scene/main/scene_tree.h b/scene/main/scene_tree.h index fc185b4f377..8ee80ebb613 100644 --- a/scene/main/scene_tree.h +++ b/scene/main/scene_tree.h @@ -103,6 +103,7 @@ private: bool debug_collisions_hint = false; bool debug_paths_hint = false; bool debug_navigation_hint = false; + bool debug_avoidance_hint = false; #endif bool paused = false; int root_lock = 0; @@ -311,6 +312,9 @@ public: void set_debug_navigation_hint(bool p_enabled); bool is_debugging_navigation_hint() const; + + void set_debug_avoidance_hint(bool p_enabled); + bool is_debugging_avoidance_hint() const; #else void set_debug_collisions_hint(bool p_enabled) {} bool is_debugging_collisions_hint() const { return false; } diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index feff4a6dc9b..e4e06a9a792 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -1142,6 +1142,10 @@ void register_scene_types() { GLOBAL_DEF_BASIC(vformat("%s/layer_%d", PNAME("layer_names/3d_navigation"), i + 1), ""); } + for (int i = 0; i < 32; i++) { + GLOBAL_DEF_BASIC(vformat("%s/layer_%d", PNAME("layer_names/avoidance"), i + 1), ""); + } + if (RenderingServer::get_singleton()) { ColorPicker::init_shaders(); // RenderingServer needs to exist for this to succeed. } diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index 6de8459e3b7..706718be192 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -115,6 +115,15 @@ static Vector2 v3_to_v2(const Vector3 &d) { return Vector2(d.x, d.z); } +static Vector vector_v2_to_v3(const Vector &d) { + Vector nd; + nd.resize(d.size()); + for (int i(0); i < nd.size(); i++) { + nd.write[i] = v2_to_v3(d[i]); + } + return nd; +} + static Vector vector_v3_to_v2(const Vector &d) { Vector nd; nd.resize(d.size()); @@ -161,6 +170,22 @@ bool NavigationServer2D::get_debug_enabled() const { } #ifdef DEBUG_ENABLED +void NavigationServer2D::set_debug_navigation_enabled(bool p_enabled) { + NavigationServer3D::get_singleton()->set_debug_navigation_enabled(p_enabled); +} + +bool NavigationServer2D::get_debug_navigation_enabled() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_enabled(); +} + +void NavigationServer2D::set_debug_avoidance_enabled(bool p_enabled) { + NavigationServer3D::get_singleton()->set_debug_avoidance_enabled(p_enabled); +} + +bool NavigationServer2D::get_debug_avoidance_enabled() const { + return NavigationServer3D::get_singleton()->get_debug_avoidance_enabled(); +} + void NavigationServer2D::set_debug_navigation_edge_connection_color(const Color &p_color) { NavigationServer3D::get_singleton()->set_debug_navigation_edge_connection_color(p_color); } @@ -264,6 +289,78 @@ void NavigationServer2D::set_debug_navigation_agent_path_point_size(real_t p_poi real_t NavigationServer2D::get_debug_navigation_agent_path_point_size() const { return NavigationServer3D::get_singleton()->get_debug_navigation_agent_path_point_size(); } + +void NavigationServer2D::set_debug_navigation_avoidance_enable_agents_radius(const bool p_value) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_enable_agents_radius(p_value); +} + +bool NavigationServer2D::get_debug_navigation_avoidance_enable_agents_radius() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_agents_radius(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_enable_obstacles_radius(const bool p_value) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_enable_obstacles_radius(p_value); +} + +bool NavigationServer2D::get_debug_navigation_avoidance_enable_obstacles_radius() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_agents_radius_color(const Color &p_color) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_agents_radius_color(p_color); +} + +Color NavigationServer2D::get_debug_navigation_avoidance_agents_radius_color() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_agents_radius_color(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_obstacles_radius_color(const Color &p_color) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_obstacles_radius_color(p_color); +} + +Color NavigationServer2D::get_debug_navigation_avoidance_obstacles_radius_color() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_obstacles_radius_color(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushin_face_color(const Color &p_color) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushin_face_color(p_color); +} + +Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushin_face_color() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_color(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushout_face_color(const Color &p_color) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushout_face_color(p_color); +} + +Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushout_face_color() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_color(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(const Color &p_color) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(p_color); +} + +Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushin_edge_color() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_color(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(const Color &p_color) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(p_color); +} + +Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushout_edge_color() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_color(); +} + +void NavigationServer2D::set_debug_navigation_avoidance_enable_obstacles_static(const bool p_value) { + NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_enable_obstacles_static(p_value); +} + +bool NavigationServer2D::get_debug_navigation_avoidance_enable_obstacles_static() const { + return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static(); +} #endif // DEBUG_ENABLED void NavigationServer2D::_bind_methods() { @@ -285,6 +382,7 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_get_links", "map"), &NavigationServer2D::map_get_links); ClassDB::bind_method(D_METHOD("map_get_regions", "map"), &NavigationServer2D::map_get_regions); ClassDB::bind_method(D_METHOD("map_get_agents", "map"), &NavigationServer2D::map_get_agents); + ClassDB::bind_method(D_METHOD("map_get_obstacles", "map"), &NavigationServer2D::map_get_obstacles); ClassDB::bind_method(D_METHOD("map_force_update", "map"), &NavigationServer2D::map_force_update); @@ -327,18 +425,31 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("link_get_owner_id", "link"), &NavigationServer2D::link_get_owner_id); ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer2D::agent_create); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_enabled", "agent", "enabled"), &NavigationServer2D::agent_set_avoidance_enabled); + ClassDB::bind_method(D_METHOD("agent_get_avoidance_enabled", "agent"), &NavigationServer2D::agent_get_avoidance_enabled); ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer2D::agent_set_map); ClassDB::bind_method(D_METHOD("agent_get_map", "agent"), &NavigationServer2D::agent_get_map); ClassDB::bind_method(D_METHOD("agent_set_neighbor_distance", "agent", "distance"), &NavigationServer2D::agent_set_neighbor_distance); ClassDB::bind_method(D_METHOD("agent_set_max_neighbors", "agent", "count"), &NavigationServer2D::agent_set_max_neighbors); - ClassDB::bind_method(D_METHOD("agent_set_time_horizon", "agent", "time"), &NavigationServer2D::agent_set_time_horizon); + ClassDB::bind_method(D_METHOD("agent_set_time_horizon_agents", "agent", "time_horizon"), &NavigationServer2D::agent_set_time_horizon_agents); + ClassDB::bind_method(D_METHOD("agent_set_time_horizon_obstacles", "agent", "time_horizon"), &NavigationServer2D::agent_set_time_horizon_obstacles); ClassDB::bind_method(D_METHOD("agent_set_radius", "agent", "radius"), &NavigationServer2D::agent_set_radius); ClassDB::bind_method(D_METHOD("agent_set_max_speed", "agent", "max_speed"), &NavigationServer2D::agent_set_max_speed); + ClassDB::bind_method(D_METHOD("agent_set_velocity_forced", "agent", "velocity"), &NavigationServer2D::agent_set_velocity_forced); ClassDB::bind_method(D_METHOD("agent_set_velocity", "agent", "velocity"), &NavigationServer2D::agent_set_velocity); - ClassDB::bind_method(D_METHOD("agent_set_target_velocity", "agent", "target_velocity"), &NavigationServer2D::agent_set_target_velocity); ClassDB::bind_method(D_METHOD("agent_set_position", "agent", "position"), &NavigationServer2D::agent_set_position); ClassDB::bind_method(D_METHOD("agent_is_map_changed", "agent"), &NavigationServer2D::agent_is_map_changed); - ClassDB::bind_method(D_METHOD("agent_set_callback", "agent", "callback"), &NavigationServer2D::agent_set_callback); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_callback", "agent", "callback"), &NavigationServer2D::agent_set_avoidance_callback); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_layers", "agent", "layers"), &NavigationServer2D::agent_set_avoidance_layers); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_mask", "agent", "mask"), &NavigationServer2D::agent_set_avoidance_mask); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_priority", "agent", "priority"), &NavigationServer2D::agent_set_avoidance_priority); + + ClassDB::bind_method(D_METHOD("obstacle_create"), &NavigationServer2D::obstacle_create); + ClassDB::bind_method(D_METHOD("obstacle_set_map", "obstacle", "map"), &NavigationServer2D::obstacle_set_map); + ClassDB::bind_method(D_METHOD("obstacle_get_map", "obstacle"), &NavigationServer2D::obstacle_get_map); + ClassDB::bind_method(D_METHOD("obstacle_set_position", "obstacle", "position"), &NavigationServer2D::obstacle_set_position); + ClassDB::bind_method(D_METHOD("obstacle_set_vertices", "obstacle", "vertices"), &NavigationServer2D::obstacle_set_vertices); + ClassDB::bind_method(D_METHOD("obstacle_set_avoidance_layers", "obstacle", "layers"), &NavigationServer2D::obstacle_set_avoidance_layers); ClassDB::bind_method(D_METHOD("free_rid", "rid"), &NavigationServer2D::free); @@ -378,6 +489,8 @@ TypedArray FORWARD_1_C(map_get_regions, RID, p_map, rid_to_rid); TypedArray FORWARD_1_C(map_get_agents, RID, p_map, rid_to_rid); +TypedArray FORWARD_1_C(map_get_obstacles, RID, p_map, rid_to_rid); + RID FORWARD_1_C(region_get_map, RID, p_region, rid_to_rid); RID FORWARD_1_C(agent_get_map, RID, p_agent, rid_to_rid); @@ -450,24 +563,43 @@ ObjectID FORWARD_1_C(link_get_owner_id, RID, p_link, rid_to_rid); RID NavigationServer2D::agent_create() { RID agent = NavigationServer3D::get_singleton()->agent_create(); - NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, true); return agent; } +void FORWARD_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled, rid_to_rid, bool_to_bool); +bool FORWARD_1_C(agent_get_avoidance_enabled, RID, p_agent, rid_to_rid); void FORWARD_2(agent_set_map, RID, p_agent, RID, p_map, rid_to_rid, rid_to_rid); void FORWARD_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_dist, rid_to_rid, real_to_real); void FORWARD_2(agent_set_max_neighbors, RID, p_agent, int, p_count, rid_to_rid, int_to_int); -void FORWARD_2(agent_set_time_horizon, RID, p_agent, real_t, p_time, rid_to_rid, real_to_real); +void FORWARD_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real); +void FORWARD_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real); void FORWARD_2(agent_set_radius, RID, p_agent, real_t, p_radius, rid_to_rid, real_to_real); void FORWARD_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed, rid_to_rid, real_to_real); +void FORWARD_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3); void FORWARD_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3); -void FORWARD_2(agent_set_target_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3); void FORWARD_2(agent_set_position, RID, p_agent, Vector2, p_position, rid_to_rid, v2_to_v3); -void FORWARD_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore, rid_to_rid, bool_to_bool); + bool FORWARD_1_C(agent_is_map_changed, RID, p_agent, rid_to_rid); -void FORWARD_2(agent_set_callback, RID, p_agent, Callable, p_callback, rid_to_rid, callable_to_callable); void FORWARD_1(free, RID, p_object, rid_to_rid); +void FORWARD_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback, rid_to_rid, callable_to_callable); + +void FORWARD_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers, rid_to_rid, uint32_to_uint32); +void FORWARD_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask, rid_to_rid, uint32_to_uint32); +void FORWARD_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority, rid_to_rid, real_to_real); + +RID NavigationServer2D::obstacle_create() { + RID obstacle = NavigationServer3D::get_singleton()->obstacle_create(); + return obstacle; +} +void FORWARD_2(obstacle_set_map, RID, p_obstacle, RID, p_map, rid_to_rid, rid_to_rid); +RID FORWARD_1_C(obstacle_get_map, RID, p_obstacle, rid_to_rid); +void FORWARD_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position, rid_to_rid, v2_to_v3); +void FORWARD_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers, rid_to_rid, uint32_to_uint32); + +void NavigationServer2D::obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) { + NavigationServer3D::get_singleton()->obstacle_set_vertices(p_obstacle, vector_v2_to_v3(p_vertices)); +} void NavigationServer2D::query_path(const Ref &p_query_parameters, Ref p_query_result) const { ERR_FAIL_COND(!p_query_parameters.is_valid()); diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index 43ef742bdb5..d6e84d73b6f 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -91,6 +91,7 @@ public: virtual TypedArray map_get_links(RID p_map) const; virtual TypedArray map_get_regions(RID p_map) const; virtual TypedArray map_get_agents(RID p_map) const; + virtual TypedArray map_get_obstacles(RID p_map) const; virtual void map_force_update(RID p_map); @@ -171,6 +172,8 @@ public: /// Put the agent in the map. virtual void agent_set_map(RID p_agent, RID p_map); virtual RID agent_get_map(RID p_agent) const; + virtual void agent_set_avoidance_enabled(RID p_agent, bool p_enabled); + virtual bool agent_get_avoidance_enabled(RID p_agent) const; /// The maximum distance (center point to /// center point) to other agents this agent @@ -197,7 +200,9 @@ public: /// other agents, but the less freedom this /// agent has in choosing its velocities. /// Must be positive. - virtual void agent_set_time_horizon(RID p_agent, real_t p_time); + + virtual void agent_set_time_horizon_agents(RID p_agent, real_t p_time_horizon); + virtual void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon); /// The radius of this agent. /// Must be non-negative. @@ -207,23 +212,33 @@ public: /// Must be non-negative. virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed); - /// Current velocity of the agent - virtual void agent_set_velocity(RID p_agent, Vector2 p_velocity); + /// forces and agent velocity change in the avoidance simulation, adds simulation instability if done recklessly + virtual void agent_set_velocity_forced(RID p_agent, Vector2 p_velocity); - /// The new target velocity. - virtual void agent_set_target_velocity(RID p_agent, Vector2 p_velocity); + /// The wanted velocity for the agent as a "suggestion" to the avoidance simulation. + /// The simulation will try to fulfil this velocity wish if possible but may change the velocity depending on other agent's and obstacles'. + virtual void agent_set_velocity(RID p_agent, Vector2 p_velocity); /// Position of the agent in world space. virtual void agent_set_position(RID p_agent, Vector2 p_position); - /// Agent ignore the Y axis and avoid collisions by moving only on the horizontal plane - virtual void agent_set_ignore_y(RID p_agent, bool p_ignore); - /// Returns true if the map got changed the previous frame. virtual bool agent_is_map_changed(RID p_agent) const; /// Callback called at the end of the RVO process - virtual void agent_set_callback(RID p_agent, Callable p_callback); + virtual void agent_set_avoidance_callback(RID p_agent, Callable p_callback); + + virtual void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers); + virtual void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask); + virtual void agent_set_avoidance_priority(RID p_agent, real_t p_priority); + + /// Creates the obstacle. + virtual RID obstacle_create(); + virtual void obstacle_set_map(RID p_obstacle, RID p_map); + virtual RID obstacle_get_map(RID p_obstacle) const; + virtual void obstacle_set_position(RID p_obstacle, Vector2 p_position); + virtual void obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices); + virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers); /// Returns a customized navigation path using a query parameters object virtual void query_path(const Ref &p_query_parameters, Ref p_query_result) const; @@ -238,6 +253,12 @@ public: bool get_debug_enabled() const; #ifdef DEBUG_ENABLED + void set_debug_navigation_enabled(bool p_enabled); + bool get_debug_navigation_enabled() const; + + void set_debug_avoidance_enabled(bool p_enabled); + bool get_debug_avoidance_enabled() const; + void set_debug_navigation_edge_connection_color(const Color &p_color); Color get_debug_navigation_edge_connection_color() const; @@ -276,6 +297,33 @@ public: void set_debug_navigation_agent_path_point_size(real_t p_point_size); real_t get_debug_navigation_agent_path_point_size() const; + + void set_debug_navigation_avoidance_enable_agents_radius(const bool p_value); + bool get_debug_navigation_avoidance_enable_agents_radius() const; + + void set_debug_navigation_avoidance_enable_obstacles_radius(const bool p_value); + bool get_debug_navigation_avoidance_enable_obstacles_radius() const; + + void set_debug_navigation_avoidance_agents_radius_color(const Color &p_color); + Color get_debug_navigation_avoidance_agents_radius_color() const; + + void set_debug_navigation_avoidance_obstacles_radius_color(const Color &p_color); + Color get_debug_navigation_avoidance_obstacles_radius_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushin_face_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushin_face_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushout_face_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushout_face_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushin_edge_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushout_edge_color() const; + + void set_debug_navigation_avoidance_enable_obstacles_static(const bool p_value); + bool get_debug_navigation_avoidance_enable_obstacles_static() const; #endif // DEBUG_ENABLED #ifdef DEBUG_ENABLED diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp index 10f5e71c917..e2da97b18ad 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -56,6 +56,7 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_get_links", "map"), &NavigationServer3D::map_get_links); ClassDB::bind_method(D_METHOD("map_get_regions", "map"), &NavigationServer3D::map_get_regions); ClassDB::bind_method(D_METHOD("map_get_agents", "map"), &NavigationServer3D::map_get_agents); + ClassDB::bind_method(D_METHOD("map_get_obstacles", "map"), &NavigationServer3D::map_get_obstacles); ClassDB::bind_method(D_METHOD("map_force_update", "map"), &NavigationServer3D::map_force_update); @@ -99,18 +100,36 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("link_get_owner_id", "link"), &NavigationServer3D::link_get_owner_id); ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer3D::agent_create); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_enabled", "agent", "enabled"), &NavigationServer3D::agent_set_avoidance_enabled); + ClassDB::bind_method(D_METHOD("agent_get_avoidance_enabled", "agent"), &NavigationServer3D::agent_get_avoidance_enabled); + ClassDB::bind_method(D_METHOD("agent_set_use_3d_avoidance", "agent", "enabled"), &NavigationServer3D::agent_set_use_3d_avoidance); + ClassDB::bind_method(D_METHOD("agent_get_use_3d_avoidance", "agent"), &NavigationServer3D::agent_get_use_3d_avoidance); + ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer3D::agent_set_map); ClassDB::bind_method(D_METHOD("agent_get_map", "agent"), &NavigationServer3D::agent_get_map); ClassDB::bind_method(D_METHOD("agent_set_neighbor_distance", "agent", "distance"), &NavigationServer3D::agent_set_neighbor_distance); ClassDB::bind_method(D_METHOD("agent_set_max_neighbors", "agent", "count"), &NavigationServer3D::agent_set_max_neighbors); - ClassDB::bind_method(D_METHOD("agent_set_time_horizon", "agent", "time"), &NavigationServer3D::agent_set_time_horizon); + ClassDB::bind_method(D_METHOD("agent_set_time_horizon_agents", "agent", "time_horizon"), &NavigationServer3D::agent_set_time_horizon_agents); + ClassDB::bind_method(D_METHOD("agent_set_time_horizon_obstacles", "agent", "time_horizon"), &NavigationServer3D::agent_set_time_horizon_obstacles); ClassDB::bind_method(D_METHOD("agent_set_radius", "agent", "radius"), &NavigationServer3D::agent_set_radius); + ClassDB::bind_method(D_METHOD("agent_set_height", "agent", "height"), &NavigationServer3D::agent_set_height); ClassDB::bind_method(D_METHOD("agent_set_max_speed", "agent", "max_speed"), &NavigationServer3D::agent_set_max_speed); + ClassDB::bind_method(D_METHOD("agent_set_velocity_forced", "agent", "velocity"), &NavigationServer3D::agent_set_velocity_forced); ClassDB::bind_method(D_METHOD("agent_set_velocity", "agent", "velocity"), &NavigationServer3D::agent_set_velocity); - ClassDB::bind_method(D_METHOD("agent_set_target_velocity", "agent", "target_velocity"), &NavigationServer3D::agent_set_target_velocity); ClassDB::bind_method(D_METHOD("agent_set_position", "agent", "position"), &NavigationServer3D::agent_set_position); ClassDB::bind_method(D_METHOD("agent_is_map_changed", "agent"), &NavigationServer3D::agent_is_map_changed); - ClassDB::bind_method(D_METHOD("agent_set_callback", "agent", "callback"), &NavigationServer3D::agent_set_callback); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_callback", "agent", "callback"), &NavigationServer3D::agent_set_avoidance_callback); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_layers", "agent", "layers"), &NavigationServer3D::agent_set_avoidance_layers); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_mask", "agent", "mask"), &NavigationServer3D::agent_set_avoidance_mask); + ClassDB::bind_method(D_METHOD("agent_set_avoidance_priority", "agent", "priority"), &NavigationServer3D::agent_set_avoidance_priority); + + ClassDB::bind_method(D_METHOD("obstacle_create"), &NavigationServer3D::obstacle_create); + ClassDB::bind_method(D_METHOD("obstacle_set_map", "obstacle", "map"), &NavigationServer3D::obstacle_set_map); + ClassDB::bind_method(D_METHOD("obstacle_get_map", "obstacle"), &NavigationServer3D::obstacle_get_map); + ClassDB::bind_method(D_METHOD("obstacle_set_height", "obstacle", "height"), &NavigationServer3D::obstacle_set_height); + ClassDB::bind_method(D_METHOD("obstacle_set_position", "obstacle", "position"), &NavigationServer3D::obstacle_set_position); + ClassDB::bind_method(D_METHOD("obstacle_set_vertices", "obstacle", "vertices"), &NavigationServer3D::obstacle_set_vertices); + ClassDB::bind_method(D_METHOD("obstacle_set_avoidance_layers", "obstacle", "layers"), &NavigationServer3D::obstacle_set_avoidance_layers); ClassDB::bind_method(D_METHOD("free_rid", "rid"), &NavigationServer3D::free); @@ -122,6 +141,7 @@ void NavigationServer3D::_bind_methods() { ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map"))); ADD_SIGNAL(MethodInfo("navigation_debug_changed")); + ADD_SIGNAL(MethodInfo("avoidance_debug_changed")); ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &NavigationServer3D::get_process_info); @@ -152,6 +172,9 @@ NavigationServer3D::NavigationServer3D() { GLOBAL_DEF_BASIC("navigation/3d/default_edge_connection_margin", 0.25); GLOBAL_DEF_BASIC("navigation/3d/default_link_connection_radius", 1.0); + GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true); + GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true); + #ifdef DEBUG_ENABLED debug_navigation_edge_connection_color = GLOBAL_DEF("debug/shapes/navigation/edge_connection_color", Color(1.0, 0.0, 1.0, 1.0)); debug_navigation_geometry_edge_color = GLOBAL_DEF("debug/shapes/navigation/geometry_edge_color", Color(0.5, 1.0, 1.0, 1.0)); @@ -172,13 +195,24 @@ NavigationServer3D::NavigationServer3D() { debug_navigation_enable_agent_paths = GLOBAL_DEF("debug/shapes/navigation/enable_agent_paths", true); debug_navigation_enable_agent_paths_xray = GLOBAL_DEF("debug/shapes/navigation/enable_agent_paths_xray", true); - debug_navigation_agent_path_point_size = GLOBAL_DEF("debug/shapes/navigation/agent_path_point_size", 4.0); + debug_navigation_avoidance_agents_radius_color = GLOBAL_DEF("debug/shapes/avoidance/agents_radius_color", Color(1.0, 1.0, 0.0, 0.25)); + debug_navigation_avoidance_obstacles_radius_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_radius_color", Color(1.0, 0.5, 0.0, 0.25)); + debug_navigation_avoidance_static_obstacle_pushin_face_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_face_pushin_color", Color(1.0, 0.0, 0.0, 0.0)); + debug_navigation_avoidance_static_obstacle_pushin_edge_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_edge_pushin_color", Color(1.0, 0.0, 0.0, 1.0)); + debug_navigation_avoidance_static_obstacle_pushout_face_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_face_pushout_color", Color(1.0, 1.0, 0.0, 0.5)); + debug_navigation_avoidance_static_obstacle_pushout_edge_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_edge_pushout_color", Color(1.0, 1.0, 0.0, 1.0)); + debug_navigation_avoidance_enable_agents_radius = GLOBAL_DEF("debug/shapes/avoidance/enable_agents_radius", true); + debug_navigation_avoidance_enable_obstacles_radius = GLOBAL_DEF("debug/shapes/avoidance/enable_obstacles_radius", true); + debug_navigation_avoidance_enable_obstacles_static = GLOBAL_DEF("debug/shapes/avoidance/enable_obstacles_static", true); + if (Engine::get_singleton()->is_editor_hint()) { // enable NavigationServer3D when in Editor or else navigation mesh edge connections are invisible // on runtime tests SceneTree has "Visible Navigation" set and main iteration takes care of this set_debug_enabled(true); + set_debug_navigation_enabled(true); + set_debug_avoidance_enabled(true); } #endif // DEBUG_ENABLED } @@ -207,11 +241,18 @@ bool NavigationServer3D::get_debug_enabled() const { #ifdef DEBUG_ENABLED void NavigationServer3D::_emit_navigation_debug_changed_signal() { - if (debug_dirty) { - debug_dirty = false; + if (navigation_debug_dirty) { + navigation_debug_dirty = false; emit_signal(SNAME("navigation_debug_changed")); } } + +void NavigationServer3D::_emit_avoidance_debug_changed_signal() { + if (avoidance_debug_dirty) { + avoidance_debug_dirty = false; + emit_signal(SNAME("avoidance_debug_changed")); + } +} #endif // DEBUG_ENABLED #ifdef DEBUG_ENABLED @@ -351,6 +392,7 @@ Ref NavigationServer3D::get_debug_navigation_agent_path_line Ref material = Ref(memnew(StandardMaterial3D)); material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + material->set_albedo(debug_navigation_agent_path_color); if (debug_navigation_enable_agent_paths_xray) { material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true); @@ -367,7 +409,6 @@ Ref NavigationServer3D::get_debug_navigation_agent_path_poin } Ref material = Ref(memnew(StandardMaterial3D)); - material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); material->set_albedo(debug_navigation_agent_path_color); material->set_flag(StandardMaterial3D::FLAG_USE_POINT_SIZE, true); material->set_point_size(debug_navigation_agent_path_point_size); @@ -380,6 +421,103 @@ Ref NavigationServer3D::get_debug_navigation_agent_path_poin return debug_navigation_agent_path_point_material; } +Ref NavigationServer3D::get_debug_navigation_avoidance_agents_radius_material() { + if (debug_navigation_avoidance_agents_radius_material.is_valid()) { + return debug_navigation_avoidance_agents_radius_material; + } + + Ref material = Ref(memnew(StandardMaterial3D)); + material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + material->set_cull_mode(StandardMaterial3D::CULL_DISABLED); + material->set_albedo(debug_navigation_avoidance_agents_radius_color); + material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 2); + + debug_navigation_avoidance_agents_radius_material = material; + return debug_navigation_avoidance_agents_radius_material; +} + +Ref NavigationServer3D::get_debug_navigation_avoidance_obstacles_radius_material() { + if (debug_navigation_avoidance_obstacles_radius_material.is_valid()) { + return debug_navigation_avoidance_obstacles_radius_material; + } + + Ref material = Ref(memnew(StandardMaterial3D)); + material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + material->set_cull_mode(StandardMaterial3D::CULL_DISABLED); + material->set_albedo(debug_navigation_avoidance_obstacles_radius_color); + material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 2); + + debug_navigation_avoidance_obstacles_radius_material = material; + return debug_navigation_avoidance_obstacles_radius_material; +} + +Ref NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushin_face_material() { + if (debug_navigation_avoidance_static_obstacle_pushin_face_material.is_valid()) { + return debug_navigation_avoidance_static_obstacle_pushin_face_material; + } + + Ref material = Ref(memnew(StandardMaterial3D)); + material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + material->set_cull_mode(StandardMaterial3D::CULL_DISABLED); + material->set_albedo(debug_navigation_avoidance_static_obstacle_pushin_face_color); + material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 2); + + debug_navigation_avoidance_static_obstacle_pushin_face_material = material; + return debug_navigation_avoidance_static_obstacle_pushin_face_material; +} + +Ref NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushout_face_material() { + if (debug_navigation_avoidance_static_obstacle_pushout_face_material.is_valid()) { + return debug_navigation_avoidance_static_obstacle_pushout_face_material; + } + + Ref material = Ref(memnew(StandardMaterial3D)); + material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + material->set_cull_mode(StandardMaterial3D::CULL_DISABLED); + material->set_albedo(debug_navigation_avoidance_static_obstacle_pushout_face_color); + material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 2); + + debug_navigation_avoidance_static_obstacle_pushout_face_material = material; + return debug_navigation_avoidance_static_obstacle_pushout_face_material; +} + +Ref NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushin_edge_material() { + if (debug_navigation_avoidance_static_obstacle_pushin_edge_material.is_valid()) { + return debug_navigation_avoidance_static_obstacle_pushin_edge_material; + } + + Ref material = Ref(memnew(StandardMaterial3D)); + material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + //material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + //material->set_cull_mode(StandardMaterial3D::CULL_DISABLED); + material->set_albedo(debug_navigation_avoidance_static_obstacle_pushin_edge_color); + //material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 2); + material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true); + + debug_navigation_avoidance_static_obstacle_pushin_edge_material = material; + return debug_navigation_avoidance_static_obstacle_pushin_edge_material; +} + +Ref NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushout_edge_material() { + if (debug_navigation_avoidance_static_obstacle_pushout_edge_material.is_valid()) { + return debug_navigation_avoidance_static_obstacle_pushout_edge_material; + } + + Ref material = Ref(memnew(StandardMaterial3D)); + material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED); + ///material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA); + //material->set_cull_mode(StandardMaterial3D::CULL_DISABLED); + material->set_albedo(debug_navigation_avoidance_static_obstacle_pushout_edge_color); + //material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 2); + material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true); + + debug_navigation_avoidance_static_obstacle_pushout_edge_material = material; + return debug_navigation_avoidance_static_obstacle_pushout_edge_material; +} + void NavigationServer3D::set_debug_navigation_edge_connection_color(const Color &p_color) { debug_navigation_edge_connection_color = p_color; if (debug_navigation_edge_connections_material.is_valid()) { @@ -484,7 +622,7 @@ Color NavigationServer3D::get_debug_navigation_agent_path_color() const { void NavigationServer3D::set_debug_navigation_enable_edge_connections(const bool p_value) { debug_navigation_enable_edge_connections = p_value; - debug_dirty = true; + navigation_debug_dirty = true; call_deferred("_emit_navigation_debug_changed_signal"); } @@ -505,7 +643,7 @@ bool NavigationServer3D::get_debug_navigation_enable_edge_connections_xray() con void NavigationServer3D::set_debug_navigation_enable_edge_lines(const bool p_value) { debug_navigation_enable_edge_lines = p_value; - debug_dirty = true; + navigation_debug_dirty = true; call_deferred("_emit_navigation_debug_changed_signal"); } @@ -526,7 +664,7 @@ bool NavigationServer3D::get_debug_navigation_enable_edge_lines_xray() const { void NavigationServer3D::set_debug_navigation_enable_geometry_face_random_color(const bool p_value) { debug_navigation_enable_geometry_face_random_color = p_value; - debug_dirty = true; + navigation_debug_dirty = true; call_deferred("_emit_navigation_debug_changed_signal"); } @@ -536,7 +674,7 @@ bool NavigationServer3D::get_debug_navigation_enable_geometry_face_random_color( void NavigationServer3D::set_debug_navigation_enable_link_connections(const bool p_value) { debug_navigation_enable_link_connections = p_value; - debug_dirty = true; + navigation_debug_dirty = true; call_deferred("_emit_navigation_debug_changed_signal"); } @@ -555,6 +693,102 @@ bool NavigationServer3D::get_debug_navigation_enable_link_connections_xray() con return debug_navigation_enable_link_connections_xray; } +void NavigationServer3D::set_debug_navigation_avoidance_enable_agents_radius(const bool p_value) { + debug_navigation_avoidance_enable_agents_radius = p_value; + avoidance_debug_dirty = true; + call_deferred("_emit_avoidance_debug_changed_signal"); +} + +bool NavigationServer3D::get_debug_navigation_avoidance_enable_agents_radius() const { + return debug_navigation_avoidance_enable_agents_radius; +} + +void NavigationServer3D::set_debug_navigation_avoidance_enable_obstacles_radius(const bool p_value) { + debug_navigation_avoidance_enable_obstacles_radius = p_value; + avoidance_debug_dirty = true; + call_deferred("_emit_avoidance_debug_changed_signal"); +} + +bool NavigationServer3D::get_debug_navigation_avoidance_enable_obstacles_radius() const { + return debug_navigation_avoidance_enable_obstacles_radius; +} + +void NavigationServer3D::set_debug_navigation_avoidance_enable_obstacles_static(const bool p_value) { + debug_navigation_avoidance_enable_obstacles_static = p_value; + avoidance_debug_dirty = true; + call_deferred("_emit_avoidance_debug_changed_signal"); +} + +bool NavigationServer3D::get_debug_navigation_avoidance_enable_obstacles_static() const { + return debug_navigation_avoidance_enable_obstacles_static; +} + +void NavigationServer3D::set_debug_navigation_avoidance_agents_radius_color(const Color &p_color) { + debug_navigation_avoidance_agents_radius_color = p_color; + if (debug_navigation_avoidance_agents_radius_material.is_valid()) { + debug_navigation_avoidance_agents_radius_material->set_albedo(debug_navigation_avoidance_agents_radius_color); + } +} + +Color NavigationServer3D::get_debug_navigation_avoidance_agents_radius_color() const { + return debug_navigation_avoidance_agents_radius_color; +} + +void NavigationServer3D::set_debug_navigation_avoidance_obstacles_radius_color(const Color &p_color) { + debug_navigation_avoidance_obstacles_radius_color = p_color; + if (debug_navigation_avoidance_obstacles_radius_material.is_valid()) { + debug_navigation_avoidance_obstacles_radius_material->set_albedo(debug_navigation_avoidance_obstacles_radius_color); + } +} + +Color NavigationServer3D::get_debug_navigation_avoidance_obstacles_radius_color() const { + return debug_navigation_avoidance_obstacles_radius_color; +} + +void NavigationServer3D::set_debug_navigation_avoidance_static_obstacle_pushin_face_color(const Color &p_color) { + debug_navigation_avoidance_static_obstacle_pushin_face_color = p_color; + if (debug_navigation_avoidance_static_obstacle_pushin_face_material.is_valid()) { + debug_navigation_avoidance_static_obstacle_pushin_face_material->set_albedo(debug_navigation_avoidance_static_obstacle_pushin_face_color); + } +} + +Color NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushin_face_color() const { + return debug_navigation_avoidance_static_obstacle_pushin_face_color; +} + +void NavigationServer3D::set_debug_navigation_avoidance_static_obstacle_pushout_face_color(const Color &p_color) { + debug_navigation_avoidance_static_obstacle_pushout_face_color = p_color; + if (debug_navigation_avoidance_static_obstacle_pushout_face_material.is_valid()) { + debug_navigation_avoidance_static_obstacle_pushout_face_material->set_albedo(debug_navigation_avoidance_static_obstacle_pushout_face_color); + } +} + +Color NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushout_face_color() const { + return debug_navigation_avoidance_static_obstacle_pushout_face_color; +} + +void NavigationServer3D::set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(const Color &p_color) { + debug_navigation_avoidance_static_obstacle_pushin_edge_color = p_color; + if (debug_navigation_avoidance_static_obstacle_pushin_edge_material.is_valid()) { + debug_navigation_avoidance_static_obstacle_pushin_edge_material->set_albedo(debug_navigation_avoidance_static_obstacle_pushin_edge_color); + } +} + +Color NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushin_edge_color() const { + return debug_navigation_avoidance_static_obstacle_pushin_edge_color; +} + +void NavigationServer3D::set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(const Color &p_color) { + debug_navigation_avoidance_static_obstacle_pushout_edge_color = p_color; + if (debug_navigation_avoidance_static_obstacle_pushout_edge_material.is_valid()) { + debug_navigation_avoidance_static_obstacle_pushout_edge_material->set_albedo(debug_navigation_avoidance_static_obstacle_pushout_edge_color); + } +} + +Color NavigationServer3D::get_debug_navigation_avoidance_static_obstacle_pushout_edge_color() const { + return debug_navigation_avoidance_static_obstacle_pushout_edge_color; +} + void NavigationServer3D::set_debug_navigation_enable_agent_paths(const bool p_value) { if (debug_navigation_enable_agent_paths != p_value) { debug_dirty = true; @@ -585,6 +819,26 @@ bool NavigationServer3D::get_debug_navigation_enable_agent_paths_xray() const { return debug_navigation_enable_agent_paths_xray; } +void NavigationServer3D::set_debug_navigation_enabled(bool p_enabled) { + debug_navigation_enabled = p_enabled; + navigation_debug_dirty = true; + call_deferred("_emit_navigation_debug_changed_signal"); +} + +bool NavigationServer3D::get_debug_navigation_enabled() const { + return debug_navigation_enabled; +} + +void NavigationServer3D::set_debug_avoidance_enabled(bool p_enabled) { + debug_avoidance_enabled = p_enabled; + avoidance_debug_dirty = true; + call_deferred("_emit_avoidance_debug_changed_signal"); +} + +bool NavigationServer3D::get_debug_avoidance_enabled() const { + return debug_avoidance_enabled; +} + #endif // DEBUG_ENABLED void NavigationServer3D::query_path(const Ref &p_query_parameters, Ref p_query_result) const { diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h index 7348dab65a8..eb8fc59041c 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -103,6 +103,7 @@ public: virtual TypedArray map_get_links(RID p_map) const = 0; virtual TypedArray map_get_regions(RID p_map) const = 0; virtual TypedArray map_get_agents(RID p_map) const = 0; + virtual TypedArray map_get_obstacles(RID p_map) const = 0; virtual void map_force_update(RID p_map) = 0; @@ -187,6 +188,12 @@ public: virtual void agent_set_map(RID p_agent, RID p_map) = 0; virtual RID agent_get_map(RID p_agent) const = 0; + virtual void agent_set_avoidance_enabled(RID p_agent, bool p_enabled) = 0; + virtual bool agent_get_avoidance_enabled(RID p_agent) const = 0; + + virtual void agent_set_use_3d_avoidance(RID p_agent, bool p_enabled) = 0; + virtual bool agent_get_use_3d_avoidance(RID p_agent) const = 0; + /// The maximum distance (center point to /// center point) to other agents this agent /// takes into account in the navigation. The @@ -204,41 +211,53 @@ public: /// be safe. virtual void agent_set_max_neighbors(RID p_agent, int p_count) = 0; - /// The minimal amount of time for which this - /// agent's velocities that are computed by the - /// simulation are safe with respect to other - /// agents. The larger this number, the sooner - /// this agent will respond to the presence of - /// other agents, but the less freedom this - /// agent has in choosing its velocities. - /// Must be positive. - virtual void agent_set_time_horizon(RID p_agent, real_t p_time) = 0; + // Sets the minimum amount of time in seconds that an agent's + // must be able to stay on the calculated velocity while still avoiding collisions with agent's + // if this value is set to high an agent will often fall back to using a very low velocity just to be safe + virtual void agent_set_time_horizon_agents(RID p_agent, real_t p_time_horizon) = 0; + + /// Sets the minimum amount of time in seconds that an agent's + // must be able to stay on the calculated velocity while still avoiding collisions with obstacle's + // if this value is set to high an agent will often fall back to using a very low velocity just to be safe + virtual void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon) = 0; /// The radius of this agent. /// Must be non-negative. virtual void agent_set_radius(RID p_agent, real_t p_radius) = 0; + virtual void agent_set_height(RID p_agent, real_t p_height) = 0; /// The maximum speed of this agent. /// Must be non-negative. virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed) = 0; - /// Current velocity of the agent - virtual void agent_set_velocity(RID p_agent, Vector3 p_velocity) = 0; + /// forces and agent velocity change in the avoidance simulation, adds simulation instability if done recklessly + virtual void agent_set_velocity_forced(RID p_agent, Vector3 p_velocity) = 0; - /// The new target velocity. - virtual void agent_set_target_velocity(RID p_agent, Vector3 p_velocity) = 0; + /// The wanted velocity for the agent as a "suggestion" to the avoidance simulation. + /// The simulation will try to fulfil this velocity wish if possible but may change the velocity depending on other agent's and obstacles'. + virtual void agent_set_velocity(RID p_agent, Vector3 p_velocity) = 0; /// Position of the agent in world space. virtual void agent_set_position(RID p_agent, Vector3 p_position) = 0; - /// Agent ignore the Y axis and avoid collisions by moving only on the horizontal plane - virtual void agent_set_ignore_y(RID p_agent, bool p_ignore) = 0; - /// Returns true if the map got changed the previous frame. virtual bool agent_is_map_changed(RID p_agent) const = 0; /// Callback called at the end of the RVO process - virtual void agent_set_callback(RID p_agent, Callable p_callback) = 0; + virtual void agent_set_avoidance_callback(RID p_agent, Callable p_callback) = 0; + + virtual void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers) = 0; + virtual void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask) = 0; + virtual void agent_set_avoidance_priority(RID p_agent, real_t p_priority) = 0; + + /// Creates the obstacle. + virtual RID obstacle_create() = 0; + virtual void obstacle_set_map(RID p_obstacle, RID p_map) = 0; + virtual RID obstacle_get_map(RID p_obstacle) const = 0; + virtual void obstacle_set_height(RID p_obstacle, real_t p_height) = 0; + virtual void obstacle_set_position(RID p_obstacle, Vector3 p_position) = 0; + virtual void obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) = 0; + virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) = 0; /// Destroy the `RID` virtual void free(RID p_object) = 0; @@ -282,8 +301,15 @@ private: #ifdef DEBUG_ENABLED bool debug_dirty = true; + + bool debug_navigation_enabled = false; + bool navigation_debug_dirty = true; void _emit_navigation_debug_changed_signal(); + bool debug_avoidance_enabled = false; + bool avoidance_debug_dirty = true; + void _emit_avoidance_debug_changed_signal(); + Color debug_navigation_edge_connection_color = Color(1.0, 0.0, 1.0, 1.0); Color debug_navigation_geometry_edge_color = Color(0.5, 1.0, 1.0, 1.0); Color debug_navigation_geometry_face_color = Color(0.5, 1.0, 1.0, 0.4); @@ -295,6 +321,14 @@ private: real_t debug_navigation_agent_path_point_size = 4.0; + Color debug_navigation_avoidance_agents_radius_color = Color(1.0, 1.0, 0.0, 0.25); + Color debug_navigation_avoidance_obstacles_radius_color = Color(1.0, 0.5, 0.0, 0.25); + + Color debug_navigation_avoidance_static_obstacle_pushin_face_color = Color(1.0, 0.0, 0.0, 0.0); + Color debug_navigation_avoidance_static_obstacle_pushout_face_color = Color(1.0, 1.0, 0.0, 0.5); + Color debug_navigation_avoidance_static_obstacle_pushin_edge_color = Color(1.0, 0.0, 0.0, 1.0); + Color debug_navigation_avoidance_static_obstacle_pushout_edge_color = Color(1.0, 1.0, 0.0, 1.0); + bool debug_navigation_enable_edge_connections = true; bool debug_navigation_enable_edge_connections_xray = true; bool debug_navigation_enable_edge_lines = true; @@ -305,6 +339,10 @@ private: bool debug_navigation_enable_agent_paths = true; bool debug_navigation_enable_agent_paths_xray = true; + bool debug_navigation_avoidance_enable_agents_radius = true; + bool debug_navigation_avoidance_enable_obstacles_radius = true; + bool debug_navigation_avoidance_enable_obstacles_static = true; + Ref debug_navigation_geometry_edge_material; Ref debug_navigation_geometry_face_material; Ref debug_navigation_geometry_edge_disabled_material; @@ -312,11 +350,24 @@ private: Ref debug_navigation_edge_connections_material; Ref debug_navigation_link_connections_material; Ref debug_navigation_link_connections_disabled_material; + Ref debug_navigation_avoidance_agents_radius_material; + Ref debug_navigation_avoidance_obstacles_radius_material; + + Ref debug_navigation_avoidance_static_obstacle_pushin_face_material; + Ref debug_navigation_avoidance_static_obstacle_pushout_face_material; + Ref debug_navigation_avoidance_static_obstacle_pushin_edge_material; + Ref debug_navigation_avoidance_static_obstacle_pushout_edge_material; Ref debug_navigation_agent_path_line_material; Ref debug_navigation_agent_path_point_material; public: + void set_debug_navigation_enabled(bool p_enabled); + bool get_debug_navigation_enabled() const; + + void set_debug_avoidance_enabled(bool p_enabled); + bool get_debug_avoidance_enabled() const; + void set_debug_navigation_edge_connection_color(const Color &p_color); Color get_debug_navigation_edge_connection_color() const; @@ -341,6 +392,24 @@ public: void set_debug_navigation_agent_path_color(const Color &p_color); Color get_debug_navigation_agent_path_color() const; + void set_debug_navigation_avoidance_agents_radius_color(const Color &p_color); + Color get_debug_navigation_avoidance_agents_radius_color() const; + + void set_debug_navigation_avoidance_obstacles_radius_color(const Color &p_color); + Color get_debug_navigation_avoidance_obstacles_radius_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushin_face_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushin_face_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushout_face_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushout_face_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushin_edge_color() const; + + void set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(const Color &p_color); + Color get_debug_navigation_avoidance_static_obstacle_pushout_edge_color() const; + void set_debug_navigation_enable_edge_connections(const bool p_value); bool get_debug_navigation_enable_edge_connections() const; @@ -371,6 +440,15 @@ public: void set_debug_navigation_agent_path_point_size(real_t p_point_size); real_t get_debug_navigation_agent_path_point_size() const; + void set_debug_navigation_avoidance_enable_agents_radius(const bool p_value); + bool get_debug_navigation_avoidance_enable_agents_radius() const; + + void set_debug_navigation_avoidance_enable_obstacles_radius(const bool p_value); + bool get_debug_navigation_avoidance_enable_obstacles_radius() const; + + void set_debug_navigation_avoidance_enable_obstacles_static(const bool p_value); + bool get_debug_navigation_avoidance_enable_obstacles_static() const; + Ref get_debug_navigation_geometry_face_material(); Ref get_debug_navigation_geometry_edge_material(); Ref get_debug_navigation_geometry_face_disabled_material(); @@ -381,6 +459,14 @@ public: Ref get_debug_navigation_agent_path_line_material(); Ref get_debug_navigation_agent_path_point_material(); + + Ref get_debug_navigation_avoidance_agents_radius_material(); + Ref get_debug_navigation_avoidance_obstacles_radius_material(); + + Ref get_debug_navigation_avoidance_static_obstacle_pushin_face_material(); + Ref get_debug_navigation_avoidance_static_obstacle_pushout_face_material(); + Ref get_debug_navigation_avoidance_static_obstacle_pushin_edge_material(); + Ref get_debug_navigation_avoidance_static_obstacle_pushout_edge_material(); #endif // DEBUG_ENABLED }; diff --git a/servers/navigation_server_3d_dummy.h b/servers/navigation_server_3d_dummy.h index fd9226e59ee..c6c0eb4b346 100644 --- a/servers/navigation_server_3d_dummy.h +++ b/servers/navigation_server_3d_dummy.h @@ -57,6 +57,7 @@ public: TypedArray map_get_links(RID p_map) const override { return TypedArray(); } TypedArray map_get_regions(RID p_map) const override { return TypedArray(); } TypedArray map_get_agents(RID p_map) const override { return TypedArray(); } + TypedArray map_get_obstacles(RID p_map) const override { return TypedArray(); } void map_force_update(RID p_map) override {} RID region_create() override { return RID(); } void region_set_enter_cost(RID p_region, real_t p_enter_cost) override {} @@ -96,17 +97,32 @@ public: RID agent_create() override { return RID(); } void agent_set_map(RID p_agent, RID p_map) override {} RID agent_get_map(RID p_agent) const override { return RID(); } + void agent_set_avoidance_enabled(RID p_agent, bool p_enabled) override {} + bool agent_get_avoidance_enabled(RID p_agent) const override { return false; } + void agent_set_use_3d_avoidance(RID p_agent, bool p_enabled) override {} + bool agent_get_use_3d_avoidance(RID p_agent) const override { return false; } void agent_set_neighbor_distance(RID p_agent, real_t p_distance) override {} void agent_set_max_neighbors(RID p_agent, int p_count) override {} - void agent_set_time_horizon(RID p_agent, real_t p_time) override {} + void agent_set_time_horizon_agents(RID p_agent, real_t p_time_horizon) override {} + void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon) override {} void agent_set_radius(RID p_agent, real_t p_radius) override {} + void agent_set_height(RID p_agent, real_t p_height) override {} void agent_set_max_speed(RID p_agent, real_t p_max_speed) override {} + void agent_set_velocity_forced(RID p_agent, Vector3 p_velocity) override {} void agent_set_velocity(RID p_agent, Vector3 p_velocity) override {} - void agent_set_target_velocity(RID p_agent, Vector3 p_velocity) override {} void agent_set_position(RID p_agent, Vector3 p_position) override {} - void agent_set_ignore_y(RID p_agent, bool p_ignore) override {} bool agent_is_map_changed(RID p_agent) const override { return false; } - void agent_set_callback(RID p_agent, Callable p_callback) override {} + void agent_set_avoidance_callback(RID p_agent, Callable p_callback) override {} + void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers) override {} + void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask) override {} + void agent_set_avoidance_priority(RID p_agent, real_t p_priority) override {} + RID obstacle_create() override { return RID(); } + void obstacle_set_map(RID p_obstacle, RID p_map) override {} + RID obstacle_get_map(RID p_obstacle) const override { return RID(); } + void obstacle_set_height(RID p_obstacle, real_t p_height) override {} + void obstacle_set_position(RID p_obstacle, Vector3 p_position) override {} + void obstacle_set_vertices(RID p_obstacle, const Vector &p_vertices) override {} + void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override {} void free(RID p_object) override {} void set_active(bool p_active) override {} void process(real_t delta_time) override {} diff --git a/thirdparty/README.md b/thirdparty/README.md index 41d18bdad00..b079e27d152 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -622,18 +622,27 @@ Files extracted from upstream source: ## rvo2 +For 2D in `rvo2_2d` folder + +- Upstream: https://github.com/snape/RVO2 +- Version: git (f7c5380235f6c9ac8d19cbf71fc94e2d4758b0a3, 2021) +- License: Apache 2.0 + +For 3D in `rvo2_3d` folder + - Upstream: https://github.com/snape/RVO2-3D - Version: git (bfc048670a4e85066e86a1f923d8ea92e3add3b2, 2021) - License: Apache 2.0 Files extracted from upstream source: -- All .cpp and .h files in the `src/` folder except for Export.h, RVO.h, RVOSimulator.cpp and RVOSimulator.h +- All .cpp and .h files in the `src/` folder except for Export.h and RVO.h - LICENSE -Important: Some files have Godot-made changes; so to enrich the features -originally proposed by this library and better integrate this library with -Godot. See the patch in the `patches` folder for details. +Important: Nearly all files have Godot-made changes and renames +to make the 2D and 3D rvo libraries compatible with each other +and solve conflicts and also enrich the feature set originally +proposed by these libraries and better integrate them with Godot. ## spirv-reflect diff --git a/thirdparty/rvo2/patches/rvo2-godot-changes.patch b/thirdparty/rvo2/patches/rvo2-godot-changes.patch deleted file mode 100644 index 16dbc203edf..00000000000 --- a/thirdparty/rvo2/patches/rvo2-godot-changes.patch +++ /dev/null @@ -1,282 +0,0 @@ -diff --git a/thirdparty/rvo2/Agent.cpp b/thirdparty/rvo2/Agent.cpp -index 5e49a3554c..b35eee9c12 100644 ---- a/thirdparty/rvo2/Agent.cpp -+++ b/thirdparty/rvo2/Agent.cpp -@@ -105,18 +105,17 @@ namespace RVO { - */ - void linearProgram4(const std::vector &planes, size_t beginPlane, float radius, Vector3 &result); - -- Agent::Agent(RVOSimulator *sim) : sim_(sim), id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { } -+ Agent::Agent() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) { } - -- void Agent::computeNeighbors() -+ void Agent::computeNeighbors(KdTree *kdTree_) - { - agentNeighbors_.clear(); -- - if (maxNeighbors_ > 0) { -- sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_); -+ kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_); - } - } - -- void Agent::computeNewVelocity() -+ void Agent::computeNewVelocity(float timeStep) - { - orcaPlanes_.clear(); - const float invTimeHorizon = 1.0f / timeHorizon_; -@@ -124,10 +123,24 @@ namespace RVO { - /* Create agent ORCA planes. */ - for (size_t i = 0; i < agentNeighbors_.size(); ++i) { - const Agent *const other = agentNeighbors_[i].second; -- const Vector3 relativePosition = other->position_ - position_; -- const Vector3 relativeVelocity = velocity_ - other->velocity_; -- const float distSq = absSq(relativePosition); -+ -+ Vector3 relativePosition = other->position_ - position_; -+ Vector3 relativeVelocity = velocity_ - other->velocity_; - const float combinedRadius = radius_ + other->radius_; -+ -+ // This is a Godot feature that allow the agents to avoid the collision -+ // by moving only on the horizontal plane relative to the player velocity. -+ if (ignore_y_) { -+ // Skip if these are in two different heights -+#define ABS(m_v) (((m_v) < 0) ? (-(m_v)) : (m_v)) -+ if (ABS(relativePosition[1]) > combinedRadius * 2) { -+ continue; -+ } -+ relativePosition[1] = 0; -+ relativeVelocity[1] = 0; -+ } -+ -+ const float distSq = absSq(relativePosition); - const float combinedRadiusSq = sqr(combinedRadius); - - Plane plane; -@@ -165,7 +178,7 @@ namespace RVO { - } - else { - /* Collision. */ -- const float invTimeStep = 1.0f / sim_->timeStep_; -+ const float invTimeStep = 1.0f / timeStep; - const Vector3 w = relativeVelocity - invTimeStep * relativePosition; - const float wLength = abs(w); - const Vector3 unitW = w / wLength; -@@ -183,6 +196,11 @@ namespace RVO { - if (planeFail < orcaPlanes_.size()) { - linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); - } -+ -+ if (ignore_y_) { -+ // Not 100% necessary, but better to have. -+ newVelocity_[1] = prefVelocity_[1]; -+ } - } - - void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) -@@ -211,12 +229,6 @@ namespace RVO { - } - } - -- void Agent::update() -- { -- velocity_ = newVelocity_; -- position_ += velocity_ * sim_->timeStep_; -- } -- - bool linearProgram1(const std::vector &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) - { - const float dotProduct = line.point * line.direction; -diff --git a/thirdparty/rvo2/Agent.h b/thirdparty/rvo2/Agent.h -index d3922ec645..45fbead2f5 100644 ---- a/thirdparty/rvo2/Agent.h -+++ b/thirdparty/rvo2/Agent.h -@@ -41,30 +41,52 @@ - #include - #include - --#include "RVOSimulator.h" - #include "Vector3.h" - -+// Note: Slightly modified to work better in Godot. -+// - The agent can be created by anyone. -+// - The simulator pointer is removed. -+// - The update function is removed. -+// - The compute velocity function now need the timeStep. -+// - Moved the `Plane` class here. -+// - Added a new parameter `ignore_y_` in the `Agent`. This parameter is used to control a godot feature that allows to avoid collisions by moving on the horizontal plane. - namespace RVO { -+ /** -+ * \brief Defines a plane. -+ */ -+ class Plane { -+ public: -+ /** -+ * \brief A point on the plane. -+ */ -+ Vector3 point; -+ -+ /** -+ * \brief The normal to the plane. -+ */ -+ Vector3 normal; -+ }; -+ - /** - * \brief Defines an agent in the simulation. - */ - class Agent { -- private: -+ public: - /** - * \brief Constructs an agent instance. - * \param sim The simulator instance. - */ -- explicit Agent(RVOSimulator *sim); -+ explicit Agent(); - - /** - * \brief Computes the neighbors of this agent. - */ -- void computeNeighbors(); -+ void computeNeighbors(class KdTree *kdTree_); - - /** - * \brief Computes the new velocity of this agent. - */ -- void computeNewVelocity(); -+ void computeNewVelocity(float timeStep); - - /** - * \brief Inserts an agent neighbor into the set of neighbors of this agent. -@@ -73,16 +95,10 @@ namespace RVO { - */ - void insertAgentNeighbor(const Agent *agent, float &rangeSq); - -- /** -- * \brief Updates the three-dimensional position and three-dimensional velocity of this agent. -- */ -- void update(); -- - Vector3 newVelocity_; - Vector3 position_; - Vector3 prefVelocity_; - Vector3 velocity_; -- RVOSimulator *sim_; - size_t id_; - size_t maxNeighbors_; - float maxSpeed_; -@@ -91,9 +107,11 @@ namespace RVO { - float timeHorizon_; - std::vector > agentNeighbors_; - std::vector orcaPlanes_; -+ /// This is a godot feature that allows the Agent to avoid collision by mooving -+ /// on the horizontal plane. -+ bool ignore_y_; - - friend class KdTree; -- friend class RVOSimulator; - }; - } - -diff --git a/thirdparty/rvo2/KdTree.cpp b/thirdparty/rvo2/KdTree.cpp -index 5e9e9777a6..c857f299df 100644 ---- a/thirdparty/rvo2/KdTree.cpp -+++ b/thirdparty/rvo2/KdTree.cpp -@@ -36,16 +36,15 @@ - - #include "Agent.h" - #include "Definitions.h" --#include "RVOSimulator.h" - - namespace RVO { - const size_t RVO3D_MAX_LEAF_SIZE = 10; - -- KdTree::KdTree(RVOSimulator *sim) : sim_(sim) { } -+ KdTree::KdTree() { } - -- void KdTree::buildAgentTree() -+ void KdTree::buildAgentTree(std::vector agents) - { -- agents_ = sim_->agents_; -+ agents_.swap(agents); - - if (!agents_.empty()) { - agentTree_.resize(2 * agents_.size() - 1); -diff --git a/thirdparty/rvo2/KdTree.h b/thirdparty/rvo2/KdTree.h -index a09384c20f..69d8920ce0 100644 ---- a/thirdparty/rvo2/KdTree.h -+++ b/thirdparty/rvo2/KdTree.h -@@ -41,6 +41,9 @@ - - #include "Vector3.h" - -+// Note: Slightly modified to work better with Godot. -+// - Removed `sim_`. -+// - KdTree things are public - namespace RVO { - class Agent; - class RVOSimulator; -@@ -49,7 +52,7 @@ namespace RVO { - * \brief Defines kd-trees for agents in the simulation. - */ - class KdTree { -- private: -+ public: - /** - * \brief Defines an agent kd-tree node. - */ -@@ -90,12 +93,12 @@ namespace RVO { - * \brief Constructs a kd-tree instance. - * \param sim The simulator instance. - */ -- explicit KdTree(RVOSimulator *sim); -+ explicit KdTree(); - - /** - * \brief Builds an agent kd-tree. - */ -- void buildAgentTree(); -+ void buildAgentTree(std::vector agents); - - void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); - -@@ -110,7 +113,6 @@ namespace RVO { - - std::vector agents_; - std::vector agentTree_; -- RVOSimulator *sim_; - - friend class Agent; - friend class RVOSimulator; -diff --git a/thirdparty/rvo2/Vector3.h b/thirdparty/rvo2/Vector3.h -index 6c3223bb87..f44e311f29 100644 ---- a/thirdparty/rvo2/Vector3.h -+++ b/thirdparty/rvo2/Vector3.h -@@ -41,7 +41,7 @@ - #include - #include - --#include "Export.h" -+#define RVO3D_EXPORT - - namespace RVO { - /** -@@ -59,17 +59,6 @@ namespace RVO { - val_[2] = 0.0f; - } - -- /** -- * \brief Constructs and initializes a three-dimensional vector from the specified three-dimensional vector. -- * \param vector The three-dimensional vector containing the xyz-coordinates. -- */ -- inline Vector3(const Vector3 &vector) -- { -- val_[0] = vector[0]; -- val_[1] = vector[1]; -- val_[2] = vector[2]; -- } -- - /** - * \brief Constructs and initializes a three-dimensional vector from the specified three-element array. - * \param val The three-element array containing the xyz-coordinates. diff --git a/thirdparty/rvo2/rvo2_2d/Agent2d.cpp b/thirdparty/rvo2/rvo2_2d/Agent2d.cpp new file mode 100644 index 00000000000..3ff95a4922f --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/Agent2d.cpp @@ -0,0 +1,594 @@ +/* + * Agent2d.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "Agent2d.h" + +#include "KdTree2d.h" +#include "Obstacle2d.h" + +namespace RVO2D { + Agent2D::Agent2D() : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { } + + void Agent2D::computeNeighbors(RVOSimulator2D *sim_) + { + obstacleNeighbors_.clear(); + float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); + sim_->kdTree_->computeObstacleNeighbors(this, rangeSq); + + agentNeighbors_.clear(); + + if (maxNeighbors_ > 0) { + rangeSq = sqr(neighborDist_); + sim_->kdTree_->computeAgentNeighbors(this, rangeSq); + } + } + + /* Search for the best new velocity. */ + void Agent2D::computeNewVelocity(RVOSimulator2D *sim_) + { + orcaLines_.clear(); + + const float invTimeHorizonObst = 1.0f / timeHorizonObst_; + + /* Create obstacle ORCA lines. */ + for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { + + const Obstacle2D *obstacle1 = obstacleNeighbors_[i].second; + const Obstacle2D *obstacle2 = obstacle1->nextObstacle_; + + const Vector2 relativePosition1 = obstacle1->point_ - position_; + const Vector2 relativePosition2 = obstacle2->point_ - position_; + + /* + * Check if velocity obstacle of obstacle is already taken care of by + * previously constructed obstacle ORCA lines. + */ + bool alreadyCovered = false; + + for (size_t j = 0; j < orcaLines_.size(); ++j) { + if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) { + alreadyCovered = true; + break; + } + } + + if (alreadyCovered) { + continue; + } + + /* Not yet covered. Check for collisions. */ + + const float distSq1 = absSq(relativePosition1); + const float distSq2 = absSq(relativePosition2); + + const float radiusSq = sqr(radius_); + + const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_; + const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector); + const float distSqLine = absSq(-relativePosition1 - s * obstacleVector); + + Line line; + + if (s < 0.0f && distSq1 <= radiusSq) { + /* Collision with left vertex. Ignore if non-convex. */ + if (obstacle1->isConvex_) { + line.point = Vector2(0.0f, 0.0f); + line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x())); + orcaLines_.push_back(line); + } + + continue; + } + else if (s > 1.0f && distSq2 <= radiusSq) { + /* Collision with right vertex. Ignore if non-convex + * or if it will be taken care of by neighoring obstace */ + if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) { + line.point = Vector2(0.0f, 0.0f); + line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x())); + orcaLines_.push_back(line); + } + + continue; + } + else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) { + /* Collision with obstacle segment. */ + line.point = Vector2(0.0f, 0.0f); + line.direction = -obstacle1->unitDir_; + orcaLines_.push_back(line); + continue; + } + + /* + * No collision. + * Compute legs. When obliquely viewed, both legs can come from a single + * vertex. Legs extend cut-off line when nonconvex vertex. + */ + + Vector2 leftLegDirection, rightLegDirection; + + if (s < 0.0f && distSqLine <= radiusSq) { + /* + * Obstacle viewed obliquely so that left vertex + * defines velocity obstacle. + */ + if (!obstacle1->isConvex_) { + /* Ignore obstacle. */ + continue; + } + + obstacle2 = obstacle1; + + const float leg1 = std::sqrt(distSq1 - radiusSq); + leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; + rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; + } + else if (s > 1.0f && distSqLine <= radiusSq) { + /* + * Obstacle viewed obliquely so that + * right vertex defines velocity obstacle. + */ + if (!obstacle2->isConvex_) { + /* Ignore obstacle. */ + continue; + } + + obstacle1 = obstacle2; + + const float leg2 = std::sqrt(distSq2 - radiusSq); + leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; + rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; + } + else { + /* Usual situation. */ + if (obstacle1->isConvex_) { + const float leg1 = std::sqrt(distSq1 - radiusSq); + leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; + } + else { + /* Left vertex non-convex; left leg extends cut-off line. */ + leftLegDirection = -obstacle1->unitDir_; + } + + if (obstacle2->isConvex_) { + const float leg2 = std::sqrt(distSq2 - radiusSq); + rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; + } + else { + /* Right vertex non-convex; right leg extends cut-off line. */ + rightLegDirection = obstacle1->unitDir_; + } + } + + /* + * Legs can never point into neighboring edge when convex vertex, + * take cutoff-line of neighboring edge instead. If velocity projected on + * "foreign" leg, no constraint is added. + */ + + const Obstacle2D *const leftNeighbor = obstacle1->prevObstacle_; + + bool isLeftLegForeign = false; + bool isRightLegForeign = false; + + if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) { + /* Left leg points into obstacle. */ + leftLegDirection = -leftNeighbor->unitDir_; + isLeftLegForeign = true; + } + + if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) { + /* Right leg points into obstacle. */ + rightLegDirection = obstacle2->unitDir_; + isRightLegForeign = true; + } + + /* Compute cut-off centers. */ + const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_); + const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_); + const Vector2 cutoffVec = rightCutoff - leftCutoff; + + /* Project current velocity on velocity obstacle. */ + + /* Check if current velocity is projected on cutoff circles. */ + const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec)); + const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection); + const float tRight = ((velocity_ - rightCutoff) * rightLegDirection); + + if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) { + /* Project on left cut-off circle. */ + const Vector2 unitW = normalize(velocity_ - leftCutoff); + + line.direction = Vector2(unitW.y(), -unitW.x()); + line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW; + orcaLines_.push_back(line); + continue; + } + else if (t > 1.0f && tRight < 0.0f) { + /* Project on right cut-off circle. */ + const Vector2 unitW = normalize(velocity_ - rightCutoff); + + line.direction = Vector2(unitW.y(), -unitW.x()); + line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW; + orcaLines_.push_back(line); + continue; + } + + /* + * Project on left leg, right leg, or cut-off line, whichever is closest + * to velocity. + */ + const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); + const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); + const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection))); + + if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { + /* Project on cut-off line. */ + line.direction = -obstacle1->unitDir_; + line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); + orcaLines_.push_back(line); + continue; + } + else if (distSqLeft <= distSqRight) { + /* Project on left leg. */ + if (isLeftLegForeign) { + continue; + } + + line.direction = leftLegDirection; + line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); + orcaLines_.push_back(line); + continue; + } + else { + /* Project on right leg. */ + if (isRightLegForeign) { + continue; + } + + line.direction = -rightLegDirection; + line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); + orcaLines_.push_back(line); + continue; + } + } + + const size_t numObstLines = orcaLines_.size(); + + const float invTimeHorizon = 1.0f / timeHorizon_; + + /* Create agent ORCA lines. */ + for (size_t i = 0; i < agentNeighbors_.size(); ++i) { + const Agent2D *const other = agentNeighbors_[i].second; + + //const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f; + //const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod; + + const Vector2 relativePosition = other->position_ - position_; + const Vector2 relativeVelocity = velocity_ - other->velocity_; + const float distSq = absSq(relativePosition); + const float combinedRadius = radius_ + other->radius_; + const float combinedRadiusSq = sqr(combinedRadius); + + Line line; + Vector2 u; + + if (distSq > combinedRadiusSq) { + /* No collision. */ + const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; + /* Vector from cutoff center to relative velocity. */ + const float wLengthSq = absSq(w); + + const float dotProduct1 = w * relativePosition; + + if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { + /* Project on cut-off circle. */ + const float wLength = std::sqrt(wLengthSq); + const Vector2 unitW = w / wLength; + + line.direction = Vector2(unitW.y(), -unitW.x()); + u = (combinedRadius * invTimeHorizon - wLength) * unitW; + } + else { + /* Project on legs. */ + const float leg = std::sqrt(distSq - combinedRadiusSq); + + if (det(relativePosition, w) > 0.0f) { + /* Project on left leg. */ + line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; + } + else { + /* Project on right leg. */ + line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; + } + + const float dotProduct2 = relativeVelocity * line.direction; + + u = dotProduct2 * line.direction - relativeVelocity; + } + } + else { + /* Collision. Project on cut-off circle of time timeStep. */ + const float invTimeStep = 1.0f / sim_->timeStep_; + + /* Vector from cutoff center to relative velocity. */ + const Vector2 w = relativeVelocity - invTimeStep * relativePosition; + + const float wLength = abs(w); + const Vector2 unitW = w / wLength; + + line.direction = Vector2(unitW.y(), -unitW.x()); + u = (combinedRadius * invTimeStep - wLength) * unitW; + } + + line.point = velocity_ + 0.5f * u; + orcaLines_.push_back(line); + } + + size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_); + + if (lineFail < orcaLines_.size()) { + linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_); + } + } + + void Agent2D::insertAgentNeighbor(const Agent2D *agent, float &rangeSq) + { + // no point processing same agent + if (this == agent) { + return; + } + // ignore other agent if layers/mask bitmasks have no matching bit + if ((avoidance_mask_ & agent->avoidance_layers_) == 0) { + return; + } + // ignore other agent if this agent is below or above + if ((elevation_ > agent->elevation_ + agent->height_) || (elevation_ + height_ < agent->elevation_)) { + return; + } + + if (avoidance_priority_ > agent->avoidance_priority_) { + return; + } + + const float distSq = absSq(position_ - agent->position_); + + if (distSq < rangeSq) { + if (agentNeighbors_.size() < maxNeighbors_) { + agentNeighbors_.push_back(std::make_pair(distSq, agent)); + } + + size_t i = agentNeighbors_.size() - 1; + + while (i != 0 && distSq < agentNeighbors_[i - 1].first) { + agentNeighbors_[i] = agentNeighbors_[i - 1]; + --i; + } + + agentNeighbors_[i] = std::make_pair(distSq, agent); + + if (agentNeighbors_.size() == maxNeighbors_) { + rangeSq = agentNeighbors_.back().first; + } + } + } + + void Agent2D::insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq) + { + const Obstacle2D *const nextObstacle = obstacle->nextObstacle_; + + // ignore obstacle if no matching layer/mask + if ((avoidance_mask_ & nextObstacle->avoidance_layers_) == 0) { + return; + } + // ignore obstacle if below or above + if ((elevation_ > obstacle->elevation_ + obstacle->height_) || (elevation_ + height_ < obstacle->elevation_)) { + return; + } + + const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_); + + if (distSq < rangeSq) { + obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle)); + + size_t i = obstacleNeighbors_.size() - 1; + + while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) { + obstacleNeighbors_[i] = obstacleNeighbors_[i - 1]; + --i; + } + + obstacleNeighbors_[i] = std::make_pair(distSq, obstacle); + } + //} + } + + void Agent2D::update(RVOSimulator2D *sim_) + { + velocity_ = newVelocity_; + position_ += velocity_ * sim_->timeStep_; + } + + bool linearProgram1(const std::vector &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) + { + const float dotProduct = lines[lineNo].point * lines[lineNo].direction; + const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point); + + if (discriminant < 0.0f) { + /* Max speed circle fully invalidates line lineNo. */ + return false; + } + + const float sqrtDiscriminant = std::sqrt(discriminant); + float tLeft = -dotProduct - sqrtDiscriminant; + float tRight = -dotProduct + sqrtDiscriminant; + + for (size_t i = 0; i < lineNo; ++i) { + const float denominator = det(lines[lineNo].direction, lines[i].direction); + const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point); + + if (std::fabs(denominator) <= RVO_EPSILON) { + /* Lines lineNo and i are (almost) parallel. */ + if (numerator < 0.0f) { + return false; + } + else { + continue; + } + } + + const float t = numerator / denominator; + + if (denominator >= 0.0f) { + /* Line i bounds line lineNo on the right. */ + tRight = std::min(tRight, t); + } + else { + /* Line i bounds line lineNo on the left. */ + tLeft = std::max(tLeft, t); + } + + if (tLeft > tRight) { + return false; + } + } + + if (directionOpt) { + /* Optimize direction. */ + if (optVelocity * lines[lineNo].direction > 0.0f) { + /* Take right extreme. */ + result = lines[lineNo].point + tRight * lines[lineNo].direction; + } + else { + /* Take left extreme. */ + result = lines[lineNo].point + tLeft * lines[lineNo].direction; + } + } + else { + /* Optimize closest point. */ + const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); + + if (t < tLeft) { + result = lines[lineNo].point + tLeft * lines[lineNo].direction; + } + else if (t > tRight) { + result = lines[lineNo].point + tRight * lines[lineNo].direction; + } + else { + result = lines[lineNo].point + t * lines[lineNo].direction; + } + } + + return true; + } + + size_t linearProgram2(const std::vector &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) + { + if (directionOpt) { + /* + * Optimize direction. Note that the optimization velocity is of unit + * length in this case. + */ + result = optVelocity * radius; + } + else if (absSq(optVelocity) > sqr(radius)) { + /* Optimize closest point and outside circle. */ + result = normalize(optVelocity) * radius; + } + else { + /* Optimize closest point and inside circle. */ + result = optVelocity; + } + + for (size_t i = 0; i < lines.size(); ++i) { + if (det(lines[i].direction, lines[i].point - result) > 0.0f) { + /* Result does not satisfy constraint i. Compute new optimal result. */ + const Vector2 tempResult = result; + + if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { + result = tempResult; + return i; + } + } + } + + return lines.size(); + } + + void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result) + { + float distance = 0.0f; + + for (size_t i = beginLine; i < lines.size(); ++i) { + if (det(lines[i].direction, lines[i].point - result) > distance) { + /* Result does not satisfy constraint of line i. */ + std::vector projLines(lines.begin(), lines.begin() + static_cast(numObstLines)); + + for (size_t j = numObstLines; j < i; ++j) { + Line line; + + float determinant = det(lines[i].direction, lines[j].direction); + + if (std::fabs(determinant) <= RVO_EPSILON) { + /* Line i and line j are parallel. */ + if (lines[i].direction * lines[j].direction > 0.0f) { + /* Line i and line j point in the same direction. */ + continue; + } + else { + /* Line i and line j point in opposite direction. */ + line.point = 0.5f * (lines[i].point + lines[j].point); + } + } + else { + line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; + } + + line.direction = normalize(lines[j].direction - lines[i].direction); + projLines.push_back(line); + } + + const Vector2 tempResult = result; + + if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) { + /* This should in principle not happen. The result is by definition + * already in the feasible region of this linear program. If it fails, + * it is due to small floating point error, and the current result is + * kept. + */ + result = tempResult; + } + + distance = det(lines[i].direction, lines[i].point - result); + } + } + } +} diff --git a/thirdparty/rvo2/rvo2_2d/Agent2d.h b/thirdparty/rvo2/rvo2_2d/Agent2d.h new file mode 100644 index 00000000000..c666c2de7ba --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/Agent2d.h @@ -0,0 +1,160 @@ +/* + * Agent2d.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO2D_AGENT_H_ +#define RVO2D_AGENT_H_ + +/** + * \file Agent2d.h + * \brief Contains the Agent class. + */ + +#include "Definitions.h" +#include "RVOSimulator2d.h" + +namespace RVO2D { + /** + * \brief Defines an agent in the simulation. + */ + class Agent2D { + public: + /** + * \brief Constructs an agent instance. + * \param sim The simulator instance. + */ + explicit Agent2D(); + + /** + * \brief Computes the neighbors of this agent. + */ + void computeNeighbors(RVOSimulator2D *sim_); + + /** + * \brief Computes the new velocity of this agent. + */ + void computeNewVelocity(RVOSimulator2D *sim_); + + /** + * \brief Inserts an agent neighbor into the set of neighbors of + * this agent. + * \param agent A pointer to the agent to be inserted. + * \param rangeSq The squared range around this agent. + */ + void insertAgentNeighbor(const Agent2D *agent, float &rangeSq); + + /** + * \brief Inserts a static obstacle neighbor into the set of neighbors + * of this agent. + * \param obstacle The number of the static obstacle to be + * inserted. + * \param rangeSq The squared range around this agent. + */ + void insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq); + + /** + * \brief Updates the two-dimensional position and two-dimensional + * velocity of this agent. + */ + void update(RVOSimulator2D *sim_); + + std::vector > agentNeighbors_; + size_t maxNeighbors_; + float maxSpeed_; + float neighborDist_; + Vector2 newVelocity_; + std::vector > obstacleNeighbors_; + std::vector orcaLines_; + Vector2 position_; + Vector2 prefVelocity_; + float radius_; + float timeHorizon_; + float timeHorizonObst_; + Vector2 velocity_; + float height_ = 0.0; + float elevation_ = 0.0; + uint32_t avoidance_layers_ = 1; + uint32_t avoidance_mask_ = 1; + float avoidance_priority_ = 1.0; + + size_t id_; + + friend class KdTree2D; + friend class RVOSimulator2D; + }; + + /** + * \relates Agent + * \brief Solves a one-dimensional linear program on a specified line + * subject to linear constraints defined by lines and a circular + * constraint. + * \param lines Lines defining the linear constraints. + * \param lineNo The specified line constraint. + * \param radius The radius of the circular constraint. + * \param optVelocity The optimization velocity. + * \param directionOpt True if the direction should be optimized. + * \param result A reference to the result of the linear program. + * \return True if successful. + */ + bool linearProgram1(const std::vector &lines, size_t lineNo, + float radius, const Vector2 &optVelocity, + bool directionOpt, Vector2 &result); + + /** + * \relates Agent + * \brief Solves a two-dimensional linear program subject to linear + * constraints defined by lines and a circular constraint. + * \param lines Lines defining the linear constraints. + * \param radius The radius of the circular constraint. + * \param optVelocity The optimization velocity. + * \param directionOpt True if the direction should be optimized. + * \param result A reference to the result of the linear program. + * \return The number of the line it fails on, and the number of lines if successful. + */ + size_t linearProgram2(const std::vector &lines, float radius, + const Vector2 &optVelocity, bool directionOpt, + Vector2 &result); + + /** + * \relates Agent + * \brief Solves a two-dimensional linear program subject to linear + * constraints defined by lines and a circular constraint. + * \param lines Lines defining the linear constraints. + * \param numObstLines Count of obstacle lines. + * \param beginLine The line on which the 2-d linear program failed. + * \param radius The radius of the circular constraint. + * \param result A reference to the result of the linear program. + */ + void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, + float radius, Vector2 &result); +} + +#endif /* RVO2D_AGENT_H_ */ diff --git a/thirdparty/rvo2/rvo2_2d/Definitions.h b/thirdparty/rvo2/rvo2_2d/Definitions.h new file mode 100644 index 00000000000..4b986fc9919 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/Definitions.h @@ -0,0 +1,109 @@ +/* + * Definitions.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO2D_DEFINITIONS_H_ +#define RVO2D_DEFINITIONS_H_ + +/** + * \file Definitions.h + * \brief Contains functions and constants used in multiple classes. + */ + +#include +#include +#include +#include +#include + +#include "Vector2.h" + +/** + * \brief A sufficiently small positive number. + */ +const float RVO_EPSILON = 0.00001f; + +namespace RVO2D { + class Agent2D; + class Obstacle2D; + class RVOSimulator2D; + + /** + * \brief Computes the squared distance from a line segment with the + * specified endpoints to a specified point. + * \param a The first endpoint of the line segment. + * \param b The second endpoint of the line segment. + * \param c The point to which the squared distance is to + * be calculated. + * \return The squared distance from the line segment to the point. + */ + inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b, + const Vector2 &c) + { + const float r = ((c - a) * (b - a)) / absSq(b - a); + + if (r < 0.0f) { + return absSq(c - a); + } + else if (r > 1.0f) { + return absSq(c - b); + } + else { + return absSq(c - (a + r * (b - a))); + } + } + + /** + * \brief Computes the signed distance from a line connecting the + * specified points to a specified point. + * \param a The first point on the line. + * \param b The second point on the line. + * \param c The point to which the signed distance is to + * be calculated. + * \return Positive when the point c lies to the left of the line ab. + */ + inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c) + { + return det(a - c, b - a); + } + + /** + * \brief Computes the square of a float. + * \param a The float to be squared. + * \return The square of the float. + */ + inline float sqr(float a) + { + return a * a; + } +} + +#endif /* RVO2D_DEFINITIONS_H_ */ diff --git a/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp new file mode 100644 index 00000000000..184bc74fe28 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/KdTree2d.cpp @@ -0,0 +1,357 @@ +/* + * KdTree2d.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "KdTree2d.h" + +#include "Agent2d.h" +#include "RVOSimulator2d.h" +#include "Obstacle2d.h" + +namespace RVO2D { + KdTree2D::KdTree2D(RVOSimulator2D *sim) : obstacleTree_(NULL), sim_(sim) { } + + KdTree2D::~KdTree2D() + { + deleteObstacleTree(obstacleTree_); + } + + void KdTree2D::buildAgentTree(std::vector agents) + { + agents_.swap(agents); + + if (!agents_.empty()) { + agentTree_.resize(2 * agents_.size() - 1); + buildAgentTreeRecursive(0, agents_.size(), 0); + } + } + + void KdTree2D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) + { + agentTree_[node].begin = begin; + agentTree_[node].end = end; + agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); + agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); + + for (size_t i = begin + 1; i < end; ++i) { + agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x()); + agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x()); + agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y()); + agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y()); + } + + if (end - begin > MAX_LEAF_SIZE) { + /* No leaf node. */ + const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY); + const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); + + size_t left = begin; + size_t right = end; + + while (left < right) { + while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) { + ++left; + } + + while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) { + --right; + } + + if (left < right) { + std::swap(agents_[left], agents_[right - 1]); + ++left; + --right; + } + } + + if (left == begin) { + ++left; + ++right; + } + + agentTree_[node].left = node + 1; + agentTree_[node].right = node + 2 * (left - begin); + + buildAgentTreeRecursive(begin, left, agentTree_[node].left); + buildAgentTreeRecursive(left, end, agentTree_[node].right); + } + } + + void KdTree2D::buildObstacleTree(std::vector obstacles) + { + deleteObstacleTree(obstacleTree_); + + obstacleTree_ = buildObstacleTreeRecursive(obstacles); + } + + + KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector &obstacles) + { + if (obstacles.empty()) { + return NULL; + } + else { + ObstacleTreeNode *const node = new ObstacleTreeNode; + + size_t optimalSplit = 0; + size_t minLeft = obstacles.size(); + size_t minRight = obstacles.size(); + + for (size_t i = 0; i < obstacles.size(); ++i) { + size_t leftSize = 0; + size_t rightSize = 0; + + const Obstacle2D *const obstacleI1 = obstacles[i]; + const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_; + + /* Compute optimal split node. */ + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + const Obstacle2D *const obstacleJ1 = obstacles[j]; + const Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + ++leftSize; + } + else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + ++rightSize; + } + else { + ++leftSize; + ++rightSize; + } + + if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { + break; + } + } + + if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { + minLeft = leftSize; + minRight = rightSize; + optimalSplit = i; + } + } + + /* Build split node. */ + std::vector leftObstacles(minLeft); + std::vector rightObstacles(minRight); + + size_t leftCounter = 0; + size_t rightCounter = 0; + const size_t i = optimalSplit; + + const Obstacle2D *const obstacleI1 = obstacles[i]; + const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_; + + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + Obstacle2D *const obstacleJ1 = obstacles[j]; + Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + leftObstacles[leftCounter++] = obstacles[j]; + } + else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + rightObstacles[rightCounter++] = obstacles[j]; + } + else { + /* Split obstacle j. */ + const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); + + const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); + + Obstacle2D *const newObstacle = new Obstacle2D(); + newObstacle->point_ = splitpoint; + newObstacle->prevObstacle_ = obstacleJ1; + newObstacle->nextObstacle_ = obstacleJ2; + newObstacle->isConvex_ = true; + newObstacle->unitDir_ = obstacleJ1->unitDir_; + + newObstacle->id_ = sim_->obstacles_.size(); + + sim_->obstacles_.push_back(newObstacle); + + obstacleJ1->nextObstacle_ = newObstacle; + obstacleJ2->prevObstacle_ = newObstacle; + + if (j1LeftOfI > 0.0f) { + leftObstacles[leftCounter++] = obstacleJ1; + rightObstacles[rightCounter++] = newObstacle; + } + else { + rightObstacles[rightCounter++] = obstacleJ1; + leftObstacles[leftCounter++] = newObstacle; + } + } + } + + node->obstacle = obstacleI1; + node->left = buildObstacleTreeRecursive(leftObstacles); + node->right = buildObstacleTreeRecursive(rightObstacles); + return node; + } + } + + void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const + { + queryAgentTreeRecursive(agent, rangeSq, 0); + } + + void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const + { + queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); + } + + void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node) + { + if (node != NULL) { + deleteObstacleTree(node->left); + deleteObstacleTree(node->right); + delete node; + } + } + + void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, size_t node) const + { + if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) { + for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { + agent->insertAgentNeighbor(agents_[i], rangeSq); + } + } + else { + const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY)); + + const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY)); + + if (distSqLeft < distSqRight) { + if (distSqLeft < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); + + if (distSqRight < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); + } + } + } + else { + if (distSqRight < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); + + if (distSqLeft < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); + } + } + } + + } + } + + void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return; + } + else { + const Obstacle2D *const obstacle1 = node->obstacle; + const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_; + + const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); + + queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); + + const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); + + if (distSqLine < rangeSq) { + if (agentLeftOfLine < 0.0f) { + /* + * Try obstacle at this node only if agent is on right side of + * obstacle (and can see obstacle). + */ + agent->insertObstacleNeighbor(node->obstacle, rangeSq); + } + + /* Try other side of line. */ + queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); + + } + } + } + + bool KdTree2D::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const + { + return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); + } + + bool KdTree2D::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return true; + } + else { + const Obstacle2D *const obstacle1 = node->obstacle; + const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_; + + const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); + const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); + const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); + + if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); + } + else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); + } + else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { + /* One can see through obstacle from left to right. */ + return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); + } + else { + const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); + const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); + const float invLengthQ = 1.0f / absSq(q2 - q1); + + return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right)); + } + } + } +} diff --git a/thirdparty/rvo2/rvo2_2d/KdTree2d.h b/thirdparty/rvo2/rvo2_2d/KdTree2d.h new file mode 100644 index 00000000000..c7159eab970 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/KdTree2d.h @@ -0,0 +1,203 @@ +/* + * KdTree2d.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO2D_KD_TREE_H_ +#define RVO2D_KD_TREE_H_ + +/** + * \file KdTree2d.h + * \brief Contains the KdTree class. + */ + +#include "Definitions.h" + +namespace RVO2D { + /** + * \brief Defines kd-trees for agents and static obstacles in the + * simulation. + */ + class KdTree2D { + public: + /** + * \brief Defines an agent kd-tree node. + */ + class AgentTreeNode { + public: + /** + * \brief The beginning node number. + */ + size_t begin; + + /** + * \brief The ending node number. + */ + size_t end; + + /** + * \brief The left node number. + */ + size_t left; + + /** + * \brief The maximum x-coordinate. + */ + float maxX; + + /** + * \brief The maximum y-coordinate. + */ + float maxY; + + /** + * \brief The minimum x-coordinate. + */ + float minX; + + /** + * \brief The minimum y-coordinate. + */ + float minY; + + /** + * \brief The right node number. + */ + size_t right; + }; + + /** + * \brief Defines an obstacle kd-tree node. + */ + class ObstacleTreeNode { + public: + /** + * \brief The left obstacle tree node. + */ + ObstacleTreeNode *left; + + /** + * \brief The obstacle number. + */ + const Obstacle2D *obstacle; + + /** + * \brief The right obstacle tree node. + */ + ObstacleTreeNode *right; + }; + + /** + * \brief Constructs a kd-tree instance. + * \param sim The simulator instance. + */ + explicit KdTree2D(RVOSimulator2D *sim); + + /** + * \brief Destroys this kd-tree instance. + */ + ~KdTree2D(); + + /** + * \brief Builds an agent kd-tree. + */ + void buildAgentTree(std::vector agents); + + void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); + + /** + * \brief Builds an obstacle kd-tree. + */ + void buildObstacleTree(std::vector obstacles); + + ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector & + obstacles); + + /** + * \brief Computes the agent neighbors of the specified agent. + * \param agent A pointer to the agent for which agent + * neighbors are to be computed. + * \param rangeSq The squared range around the agent. + */ + void computeAgentNeighbors(Agent2D *agent, float &rangeSq) const; + + /** + * \brief Computes the obstacle neighbors of the specified agent. + * \param agent A pointer to the agent for which obstacle + * neighbors are to be computed. + * \param rangeSq The squared range around the agent. + */ + void computeObstacleNeighbors(Agent2D *agent, float rangeSq) const; + + /** + * \brief Deletes the specified obstacle tree node. + * \param node A pointer to the obstacle tree node to be + * deleted. + */ + void deleteObstacleTree(ObstacleTreeNode *node); + + void queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, + size_t node) const; + + void queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, + const ObstacleTreeNode *node) const; + + /** + * \brief Queries the visibility between two points within a + * specified radius. + * \param q1 The first point between which visibility is + * to be tested. + * \param q2 The second point between which visibility is + * to be tested. + * \param radius The radius within which visibility is to be + * tested. + * \return True if q1 and q2 are mutually visible within the radius; + * false otherwise. + */ + bool queryVisibility(const Vector2 &q1, const Vector2 &q2, + float radius) const; + + bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, + float radius, + const ObstacleTreeNode *node) const; + + std::vector agents_; + std::vector agentTree_; + ObstacleTreeNode *obstacleTree_; + RVOSimulator2D *sim_; + + static const size_t MAX_LEAF_SIZE = 10; + + friend class Agent2D; + friend class RVOSimulator2D; + }; +} + +#endif /* RVO2D_KD_TREE_H_ */ diff --git a/thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp b/thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp new file mode 100644 index 00000000000..a80c8af1366 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp @@ -0,0 +1,38 @@ +/* + * Obstacle2d.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "Obstacle2d.h" +#include "RVOSimulator2d.h" + +namespace RVO2D { + Obstacle2D::Obstacle2D() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { } +} diff --git a/thirdparty/rvo2/rvo2_2d/Obstacle2d.h b/thirdparty/rvo2/rvo2_2d/Obstacle2d.h new file mode 100644 index 00000000000..9ba59370535 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/Obstacle2d.h @@ -0,0 +1,72 @@ +/* + * Obstacle2d.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO2D_OBSTACLE_H_ +#define RVO2D_OBSTACLE_H_ + +/** + * \file Obstacle2d.h + * \brief Contains the Obstacle class. + */ + +#include "Definitions.h" + +namespace RVO2D { + /** + * \brief Defines static obstacles in the simulation. + */ + class Obstacle2D { + public: + /** + * \brief Constructs a static obstacle instance. + */ + Obstacle2D(); + + bool isConvex_; + Obstacle2D *nextObstacle_; + Vector2 point_; + Obstacle2D *prevObstacle_; + Vector2 unitDir_; + + float height_ = 1.0; + float elevation_ = 0.0; + uint32_t avoidance_layers_ = 1; + + size_t id_; + + friend class Agent2D; + friend class KdTree2D; + friend class RVOSimulator2D; + }; +} + +#endif /* RVO2D_OBSTACLE_H_ */ diff --git a/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp new file mode 100644 index 00000000000..9fb1555ebc5 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp @@ -0,0 +1,363 @@ +/* + * RVOSimulator2d.cpp + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "RVOSimulator2d.h" + +#include "Agent2d.h" +#include "KdTree2d.h" +#include "Obstacle2d.h" + +#ifdef _OPENMP +#include +#endif + +namespace RVO2D { + RVOSimulator2D::RVOSimulator2D() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f) + { + kdTree_ = new KdTree2D(this); + } + + RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep) + { + kdTree_ = new KdTree2D(this); + defaultAgent_ = new Agent2D(); + + defaultAgent_->maxNeighbors_ = maxNeighbors; + defaultAgent_->maxSpeed_ = maxSpeed; + defaultAgent_->neighborDist_ = neighborDist; + defaultAgent_->radius_ = radius; + defaultAgent_->timeHorizon_ = timeHorizon; + defaultAgent_->timeHorizonObst_ = timeHorizonObst; + defaultAgent_->velocity_ = velocity; + } + + RVOSimulator2D::~RVOSimulator2D() + { + if (defaultAgent_ != NULL) { + delete defaultAgent_; + } + + for (size_t i = 0; i < agents_.size(); ++i) { + delete agents_[i]; + } + + for (size_t i = 0; i < obstacles_.size(); ++i) { + delete obstacles_[i]; + } + + delete kdTree_; + } + + size_t RVOSimulator2D::addAgent(const Vector2 &position) + { + if (defaultAgent_ == NULL) { + return RVO2D_ERROR; + } + + Agent2D *agent = new Agent2D(); + + agent->position_ = position; + agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; + agent->maxSpeed_ = defaultAgent_->maxSpeed_; + agent->neighborDist_ = defaultAgent_->neighborDist_; + agent->radius_ = defaultAgent_->radius_; + agent->timeHorizon_ = defaultAgent_->timeHorizon_; + agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; + agent->velocity_ = defaultAgent_->velocity_; + + agent->id_ = agents_.size(); + + agents_.push_back(agent); + + return agents_.size() - 1; + } + + size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) + { + Agent2D *agent = new Agent2D(); + + agent->position_ = position; + agent->maxNeighbors_ = maxNeighbors; + agent->maxSpeed_ = maxSpeed; + agent->neighborDist_ = neighborDist; + agent->radius_ = radius; + agent->timeHorizon_ = timeHorizon; + agent->timeHorizonObst_ = timeHorizonObst; + agent->velocity_ = velocity; + + agent->id_ = agents_.size(); + + agents_.push_back(agent); + + return agents_.size() - 1; + } + + size_t RVOSimulator2D::addObstacle(const std::vector &vertices) + { + if (vertices.size() < 2) { + return RVO2D_ERROR; + } + + const size_t obstacleNo = obstacles_.size(); + + for (size_t i = 0; i < vertices.size(); ++i) { + Obstacle2D *obstacle = new Obstacle2D(); + obstacle->point_ = vertices[i]; + + if (i != 0) { + obstacle->prevObstacle_ = obstacles_.back(); + obstacle->prevObstacle_->nextObstacle_ = obstacle; + } + + if (i == vertices.size() - 1) { + obstacle->nextObstacle_ = obstacles_[obstacleNo]; + obstacle->nextObstacle_->prevObstacle_ = obstacle; + } + + obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]); + + if (vertices.size() == 2) { + obstacle->isConvex_ = true; + } + else { + obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); + } + + obstacle->id_ = obstacles_.size(); + + obstacles_.push_back(obstacle); + } + + return obstacleNo; + } + + void RVOSimulator2D::doStep() + { + kdTree_->buildAgentTree(agents_); + + for (int i = 0; i < static_cast(agents_.size()); ++i) { + agents_[i]->computeNeighbors(this); + agents_[i]->computeNewVelocity(this); + } + + for (int i = 0; i < static_cast(agents_.size()); ++i) { + agents_[i]->update(this); + } + + globalTime_ += timeStep_; + } + + size_t RVOSimulator2D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const + { + return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; + } + + size_t RVOSimulator2D::getAgentMaxNeighbors(size_t agentNo) const + { + return agents_[agentNo]->maxNeighbors_; + } + + float RVOSimulator2D::getAgentMaxSpeed(size_t agentNo) const + { + return agents_[agentNo]->maxSpeed_; + } + + float RVOSimulator2D::getAgentNeighborDist(size_t agentNo) const + { + return agents_[agentNo]->neighborDist_; + } + + size_t RVOSimulator2D::getAgentNumAgentNeighbors(size_t agentNo) const + { + return agents_[agentNo]->agentNeighbors_.size(); + } + + size_t RVOSimulator2D::getAgentNumObstacleNeighbors(size_t agentNo) const + { + return agents_[agentNo]->obstacleNeighbors_.size(); + } + + size_t RVOSimulator2D::getAgentNumORCALines(size_t agentNo) const + { + return agents_[agentNo]->orcaLines_.size(); + } + + size_t RVOSimulator2D::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const + { + return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_; + } + + const Line &RVOSimulator2D::getAgentORCALine(size_t agentNo, size_t lineNo) const + { + return agents_[agentNo]->orcaLines_[lineNo]; + } + + const Vector2 &RVOSimulator2D::getAgentPosition(size_t agentNo) const + { + return agents_[agentNo]->position_; + } + + const Vector2 &RVOSimulator2D::getAgentPrefVelocity(size_t agentNo) const + { + return agents_[agentNo]->prefVelocity_; + } + + float RVOSimulator2D::getAgentRadius(size_t agentNo) const + { + return agents_[agentNo]->radius_; + } + + float RVOSimulator2D::getAgentTimeHorizon(size_t agentNo) const + { + return agents_[agentNo]->timeHorizon_; + } + + float RVOSimulator2D::getAgentTimeHorizonObst(size_t agentNo) const + { + return agents_[agentNo]->timeHorizonObst_; + } + + const Vector2 &RVOSimulator2D::getAgentVelocity(size_t agentNo) const + { + return agents_[agentNo]->velocity_; + } + + float RVOSimulator2D::getGlobalTime() const + { + return globalTime_; + } + + size_t RVOSimulator2D::getNumAgents() const + { + return agents_.size(); + } + + size_t RVOSimulator2D::getNumObstacleVertices() const + { + return obstacles_.size(); + } + + const Vector2 &RVOSimulator2D::getObstacleVertex(size_t vertexNo) const + { + return obstacles_[vertexNo]->point_; + } + + size_t RVOSimulator2D::getNextObstacleVertexNo(size_t vertexNo) const + { + return obstacles_[vertexNo]->nextObstacle_->id_; + } + + size_t RVOSimulator2D::getPrevObstacleVertexNo(size_t vertexNo) const + { + return obstacles_[vertexNo]->prevObstacle_->id_; + } + + float RVOSimulator2D::getTimeStep() const + { + return timeStep_; + } + + void RVOSimulator2D::processObstacles() + { + kdTree_->buildObstacleTree(obstacles_); + } + + bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const + { + return kdTree_->queryVisibility(point1, point2, radius); + } + + void RVOSimulator2D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) + { + if (defaultAgent_ == NULL) { + defaultAgent_ = new Agent2D(); + } + + defaultAgent_->maxNeighbors_ = maxNeighbors; + defaultAgent_->maxSpeed_ = maxSpeed; + defaultAgent_->neighborDist_ = neighborDist; + defaultAgent_->radius_ = radius; + defaultAgent_->timeHorizon_ = timeHorizon; + defaultAgent_->timeHorizonObst_ = timeHorizonObst; + defaultAgent_->velocity_ = velocity; + } + + void RVOSimulator2D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) + { + agents_[agentNo]->maxNeighbors_ = maxNeighbors; + } + + void RVOSimulator2D::setAgentMaxSpeed(size_t agentNo, float maxSpeed) + { + agents_[agentNo]->maxSpeed_ = maxSpeed; + } + + void RVOSimulator2D::setAgentNeighborDist(size_t agentNo, float neighborDist) + { + agents_[agentNo]->neighborDist_ = neighborDist; + } + + void RVOSimulator2D::setAgentPosition(size_t agentNo, const Vector2 &position) + { + agents_[agentNo]->position_ = position; + } + + void RVOSimulator2D::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity) + { + agents_[agentNo]->prefVelocity_ = prefVelocity; + } + + void RVOSimulator2D::setAgentRadius(size_t agentNo, float radius) + { + agents_[agentNo]->radius_ = radius; + } + + void RVOSimulator2D::setAgentTimeHorizon(size_t agentNo, float timeHorizon) + { + agents_[agentNo]->timeHorizon_ = timeHorizon; + } + + void RVOSimulator2D::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) + { + agents_[agentNo]->timeHorizonObst_ = timeHorizonObst; + } + + void RVOSimulator2D::setAgentVelocity(size_t agentNo, const Vector2 &velocity) + { + agents_[agentNo]->velocity_ = velocity; + } + + void RVOSimulator2D::setTimeStep(float timeStep) + { + timeStep_ = timeStep; + } +} diff --git a/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h new file mode 100644 index 00000000000..e074e0fe0e8 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h @@ -0,0 +1,592 @@ +/* + * RVOSimulator2d.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO2D_RVO_SIMULATOR_H_ +#define RVO2D_RVO_SIMULATOR_H_ + +/** + * \file RVOSimulator2d.h + * \brief Contains the RVOSimulator2D class. + */ + +#include +#include +#include + +#include "Vector2.h" + +namespace RVO2D { + /** + * \brief Error value. + * + * A value equal to the largest unsigned integer that is returned in case + * of an error by functions in RVO2D::RVOSimulator2D. + */ + const size_t RVO2D_ERROR = std::numeric_limits::max(); + + /** + * \brief Defines a directed line. + */ + class Line { + public: + /** + * \brief A point on the directed line. + */ + Vector2 point; + + /** + * \brief The direction of the directed line. + */ + Vector2 direction; + }; + + class Agent2D; + class KdTree2D; + class Obstacle2D; + + /** + * \brief Defines the simulation. + * + * The main class of the library that contains all simulation functionality. + */ + class RVOSimulator2D { + public: + /** + * \brief Constructs a simulator instance. + */ + RVOSimulator2D(); + + /** + * \brief Constructs a simulator instance and sets the default + * properties for any new agent that is added. + * \param timeStep The time step of the simulation. + * Must be positive. + * \param neighborDist The default maximum distance (center point + * to center point) to other agents a new agent + * takes into account in the navigation. The + * larger this number, the longer he running + * time of the simulation. If the number is too + * low, the simulation will not be safe. Must be + * non-negative. + * \param maxNeighbors The default maximum number of other agents a + * new 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. + * \param timeHorizon The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * other agents. The larger this number, the + * sooner an agent will respond to the presence + * of other agents, but the less freedom the + * agent has in choosing its velocities. + * Must be positive. + * \param timeHorizonObst The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * obstacles. The larger this number, the + * sooner an agent will respond to the presence + * of obstacles, but the less freedom the agent + * has in choosing its velocities. + * Must be positive. + * \param radius The default radius of a new agent. + * Must be non-negative. + * \param maxSpeed The default maximum speed of a new agent. + * Must be non-negative. + * \param velocity The default initial two-dimensional linear + * velocity of a new agent (optional). + */ + RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors, + float timeHorizon, float timeHorizonObst, float radius, + float maxSpeed, const Vector2 &velocity = Vector2()); + + /** + * \brief Destroys this simulator instance. + */ + ~RVOSimulator2D(); + + /** + * \brief Adds a new agent with default properties to the + * simulation. + * \param position The two-dimensional starting position of + * this agent. + * \return The number of the agent, or RVO2D::RVO2D_ERROR when the agent + * defaults have not been set. + */ + size_t addAgent(const Vector2 &position); + + /** + * \brief Adds a new agent to the simulation. + * \param position The two-dimensional starting position of + * this agent. + * \param neighborDist The maximum distance (center point to + * center point) to other agents this agent + * takes into account in the navigation. The + * larger this number, the longer the running + * time of the simulation. If the number is too + * low, the simulation will not be safe. + * Must be non-negative. + * \param maxNeighbors The maximum number of other agents this + * agent takes into account in the navigation. + * The larger this number, the longer the + * running time of the simulation. If the + * number is too low, the simulation will not + * be safe. + * \param timeHorizon The minimal amount of time for which this + * agent's velocities that are computed by the + * simulation are safe with respect to other + * agents. The larger this number, the sooner + * this agent will respond to the presence of + * other agents, but the less freedom this + * agent has in choosing its velocities. + * Must be positive. + * \param timeHorizonObst The minimal amount of time for which this + * agent's velocities that are computed by the + * simulation are safe with respect to + * obstacles. The larger this number, the + * sooner this agent will respond to the + * presence of obstacles, but the less freedom + * this agent has in choosing its velocities. + * Must be positive. + * \param radius The radius of this agent. + * Must be non-negative. + * \param maxSpeed The maximum speed of this agent. + * Must be non-negative. + * \param velocity The initial two-dimensional linear velocity + * of this agent (optional). + * \return The number of the agent. + */ + size_t addAgent(const Vector2 &position, float neighborDist, + size_t maxNeighbors, float timeHorizon, + float timeHorizonObst, float radius, float maxSpeed, + const Vector2 &velocity = Vector2()); + + /** + * \brief Adds a new obstacle to the simulation. + * \param vertices List of the vertices of the polygonal + * obstacle in counterclockwise order. + * \return The number of the first vertex of the obstacle, + * or RVO2D::RVO2D_ERROR when the number of vertices is less than two. + * \note To add a "negative" obstacle, e.g. a bounding polygon around + * the environment, the vertices should be listed in clockwise + * order. + */ + size_t addObstacle(const std::vector &vertices); + + /** + * \brief Lets the simulator perform a simulation step and updates the + * two-dimensional position and two-dimensional velocity of + * each agent. + */ + void doStep(); + + /** + * \brief Returns the specified agent neighbor of the specified + * agent. + * \param agentNo The number of the agent whose agent + * neighbor is to be retrieved. + * \param neighborNo The number of the agent neighbor to be + * retrieved. + * \return The number of the neighboring agent. + */ + size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const; + + /** + * \brief Returns the maximum neighbor count of a specified agent. + * \param agentNo The number of the agent whose maximum + * neighbor count is to be retrieved. + * \return The present maximum neighbor count of the agent. + */ + size_t getAgentMaxNeighbors(size_t agentNo) const; + + /** + * \brief Returns the maximum speed of a specified agent. + * \param agentNo The number of the agent whose maximum speed + * is to be retrieved. + * \return The present maximum speed of the agent. + */ + float getAgentMaxSpeed(size_t agentNo) const; + + /** + * \brief Returns the maximum neighbor distance of a specified + * agent. + * \param agentNo The number of the agent whose maximum + * neighbor distance is to be retrieved. + * \return The present maximum neighbor distance of the agent. + */ + float getAgentNeighborDist(size_t agentNo) const; + + /** + * \brief Returns the count of agent neighbors taken into account to + * compute the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of agent + * neighbors is to be retrieved. + * \return The count of agent neighbors taken into account to compute + * the current velocity for the specified agent. + */ + size_t getAgentNumAgentNeighbors(size_t agentNo) const; + + /** + * \brief Returns the count of obstacle neighbors taken into account + * to compute the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of + * obstacle neighbors is to be retrieved. + * \return The count of obstacle neighbors taken into account to + * compute the current velocity for the specified agent. + */ + size_t getAgentNumObstacleNeighbors(size_t agentNo) const; + + + /** + * \brief Returns the count of ORCA constraints used to compute + * the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of ORCA + * constraints is to be retrieved. + * \return The count of ORCA constraints used to compute the current + * velocity for the specified agent. + */ + size_t getAgentNumORCALines(size_t agentNo) const; + + /** + * \brief Returns the specified obstacle neighbor of the specified + * agent. + * \param agentNo The number of the agent whose obstacle + * neighbor is to be retrieved. + * \param neighborNo The number of the obstacle neighbor to be + * retrieved. + * \return The number of the first vertex of the neighboring obstacle + * edge. + */ + size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const; + + /** + * \brief Returns the specified ORCA constraint of the specified + * agent. + * \param agentNo The number of the agent whose ORCA + * constraint is to be retrieved. + * \param lineNo The number of the ORCA constraint to be + * retrieved. + * \return A line representing the specified ORCA constraint. + * \note The halfplane to the left of the line is the region of + * permissible velocities with respect to the specified + * ORCA constraint. + */ + const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const; + + /** + * \brief Returns the two-dimensional position of a specified + * agent. + * \param agentNo The number of the agent whose + * two-dimensional position is to be retrieved. + * \return The present two-dimensional position of the (center of the) + * agent. + */ + const Vector2 &getAgentPosition(size_t agentNo) const; + + /** + * \brief Returns the two-dimensional preferred velocity of a + * specified agent. + * \param agentNo The number of the agent whose + * two-dimensional preferred velocity is to be + * retrieved. + * \return The present two-dimensional preferred velocity of the agent. + */ + const Vector2 &getAgentPrefVelocity(size_t agentNo) const; + + /** + * \brief Returns the radius of a specified agent. + * \param agentNo The number of the agent whose radius is to + * be retrieved. + * \return The present radius of the agent. + */ + float getAgentRadius(size_t agentNo) const; + + /** + * \brief Returns the time horizon of a specified agent. + * \param agentNo The number of the agent whose time horizon + * is to be retrieved. + * \return The present time horizon of the agent. + */ + float getAgentTimeHorizon(size_t agentNo) const; + + /** + * \brief Returns the time horizon with respect to obstacles of a + * specified agent. + * \param agentNo The number of the agent whose time horizon + * with respect to obstacles is to be + * retrieved. + * \return The present time horizon with respect to obstacles of the + * agent. + */ + float getAgentTimeHorizonObst(size_t agentNo) const; + + /** + * \brief Returns the two-dimensional linear velocity of a + * specified agent. + * \param agentNo The number of the agent whose + * two-dimensional linear velocity is to be + * retrieved. + * \return The present two-dimensional linear velocity of the agent. + */ + const Vector2 &getAgentVelocity(size_t agentNo) const; + + /** + * \brief Returns the global time of the simulation. + * \return The present global time of the simulation (zero initially). + */ + float getGlobalTime() const; + + /** + * \brief Returns the count of agents in the simulation. + * \return The count of agents in the simulation. + */ + size_t getNumAgents() const; + + /** + * \brief Returns the count of obstacle vertices in the simulation. + * \return The count of obstacle vertices in the simulation. + */ + size_t getNumObstacleVertices() const; + + /** + * \brief Returns the two-dimensional position of a specified obstacle + * vertex. + * \param vertexNo The number of the obstacle vertex to be + * retrieved. + * \return The two-dimensional position of the specified obstacle + * vertex. + */ + const Vector2 &getObstacleVertex(size_t vertexNo) const; + + /** + * \brief Returns the number of the obstacle vertex succeeding the + * specified obstacle vertex in its polygon. + * \param vertexNo The number of the obstacle vertex whose + * successor is to be retrieved. + * \return The number of the obstacle vertex succeeding the specified + * obstacle vertex in its polygon. + */ + size_t getNextObstacleVertexNo(size_t vertexNo) const; + + /** + * \brief Returns the number of the obstacle vertex preceding the + * specified obstacle vertex in its polygon. + * \param vertexNo The number of the obstacle vertex whose + * predecessor is to be retrieved. + * \return The number of the obstacle vertex preceding the specified + * obstacle vertex in its polygon. + */ + size_t getPrevObstacleVertexNo(size_t vertexNo) const; + + /** + * \brief Returns the time step of the simulation. + * \return The present time step of the simulation. + */ + float getTimeStep() const; + + /** + * \brief Processes the obstacles that have been added so that they + * are accounted for in the simulation. + * \note Obstacles added to the simulation after this function has + * been called are not accounted for in the simulation. + */ + void processObstacles(); + + /** + * \brief Performs a visibility query between the two specified + * points with respect to the obstacles + * \param point1 The first point of the query. + * \param point2 The second point of the query. + * \param radius The minimal distance between the line + * connecting the two points and the obstacles + * in order for the points to be mutually + * visible (optional). Must be non-negative. + * \return A boolean specifying whether the two points are mutually + * visible. Returns true when the obstacles have not been + * processed. + */ + bool queryVisibility(const Vector2 &point1, const Vector2 &point2, + float radius = 0.0f) const; + + /** + * \brief Sets the default properties for any new agent that is + * added. + * \param neighborDist The default maximum distance (center point + * to center point) to other agents a new agent + * takes into account in the navigation. The + * larger this number, the longer he running + * time of the simulation. If the number is too + * low, the simulation will not be safe. + * Must be non-negative. + * \param maxNeighbors The default maximum number of other agents a + * new 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. + * \param timeHorizon The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * other agents. The larger this number, the + * sooner an agent will respond to the presence + * of other agents, but the less freedom the + * agent has in choosing its velocities. + * Must be positive. + * \param timeHorizonObst The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * obstacles. The larger this number, the + * sooner an agent will respond to the presence + * of obstacles, but the less freedom the agent + * has in choosing its velocities. + * Must be positive. + * \param radius The default radius of a new agent. + * Must be non-negative. + * \param maxSpeed The default maximum speed of a new agent. + * Must be non-negative. + * \param velocity The default initial two-dimensional linear + * velocity of a new agent (optional). + */ + void setAgentDefaults(float neighborDist, size_t maxNeighbors, + float timeHorizon, float timeHorizonObst, + float radius, float maxSpeed, + const Vector2 &velocity = Vector2()); + + /** + * \brief Sets the maximum neighbor count of a specified agent. + * \param agentNo The number of the agent whose maximum + * neighbor count is to be modified. + * \param maxNeighbors The replacement maximum neighbor count. + */ + void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors); + + /** + * \brief Sets the maximum speed of a specified agent. + * \param agentNo The number of the agent whose maximum speed + * is to be modified. + * \param maxSpeed The replacement maximum speed. Must be + * non-negative. + */ + void setAgentMaxSpeed(size_t agentNo, float maxSpeed); + + /** + * \brief Sets the maximum neighbor distance of a specified agent. + * \param agentNo The number of the agent whose maximum + * neighbor distance is to be modified. + * \param neighborDist The replacement maximum neighbor distance. + * Must be non-negative. + */ + void setAgentNeighborDist(size_t agentNo, float neighborDist); + + /** + * \brief Sets the two-dimensional position of a specified agent. + * \param agentNo The number of the agent whose + * two-dimensional position is to be modified. + * \param position The replacement of the two-dimensional + * position. + */ + void setAgentPosition(size_t agentNo, const Vector2 &position); + + /** + * \brief Sets the two-dimensional preferred velocity of a + * specified agent. + * \param agentNo The number of the agent whose + * two-dimensional preferred velocity is to be + * modified. + * \param prefVelocity The replacement of the two-dimensional + * preferred velocity. + */ + void setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity); + + /** + * \brief Sets the radius of a specified agent. + * \param agentNo The number of the agent whose radius is to + * be modified. + * \param radius The replacement radius. + * Must be non-negative. + */ + void setAgentRadius(size_t agentNo, float radius); + + /** + * \brief Sets the time horizon of a specified agent with respect + * to other agents. + * \param agentNo The number of the agent whose time horizon + * is to be modified. + * \param timeHorizon The replacement time horizon with respect + * to other agents. Must be positive. + */ + void setAgentTimeHorizon(size_t agentNo, float timeHorizon); + + /** + * \brief Sets the time horizon of a specified agent with respect + * to obstacles. + * \param agentNo The number of the agent whose time horizon + * with respect to obstacles is to be modified. + * \param timeHorizonObst The replacement time horizon with respect to + * obstacles. Must be positive. + */ + void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst); + + /** + * \brief Sets the two-dimensional linear velocity of a specified + * agent. + * \param agentNo The number of the agent whose + * two-dimensional linear velocity is to be + * modified. + * \param velocity The replacement two-dimensional linear + * velocity. + */ + void setAgentVelocity(size_t agentNo, const Vector2 &velocity); + + /** + * \brief Sets the time step of the simulation. + * \param timeStep The time step of the simulation. + * Must be positive. + */ + void setTimeStep(float timeStep); + + public: + std::vector agents_; + Agent2D *defaultAgent_; + float globalTime_; + KdTree2D *kdTree_; + std::vector obstacles_; + float timeStep_; + + friend class Agent2D; + friend class KdTree2D; + friend class Obstacle2D; + }; +} + +#endif /* RVO2D_RVO_SIMULATOR_H_ */ diff --git a/thirdparty/rvo2/rvo2_2d/Vector2.h b/thirdparty/rvo2/rvo2_2d/Vector2.h new file mode 100644 index 00000000000..24353e09f38 --- /dev/null +++ b/thirdparty/rvo2/rvo2_2d/Vector2.h @@ -0,0 +1,346 @@ +/* + * Vector2.h + * RVO2 Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_VECTOR2_H_ +#define RVO_VECTOR2_H_ + +/** + * \file Vector2.h + * \brief Contains the Vector2 class. + */ + +#include +#include + +namespace RVO2D { + /** + * \brief Defines a two-dimensional vector. + */ + class Vector2 { + public: + /** + * \brief Constructs and initializes a two-dimensional vector instance + * to (0.0, 0.0). + */ + inline Vector2() : x_(0.0f), y_(0.0f) { } + + /** + * \brief Constructs and initializes a two-dimensional vector from + * the specified xy-coordinates. + * \param x The x-coordinate of the two-dimensional + * vector. + * \param y The y-coordinate of the two-dimensional + * vector. + */ + inline Vector2(float x, float y) : x_(x), y_(y) { } + + inline Vector2(const Vector2 &vector) + { + x_ = vector.x(); + y_ = vector.y(); + } + + /** + * \brief Returns the x-coordinate of this two-dimensional vector. + * \return The x-coordinate of the two-dimensional vector. + */ + inline float x() const { return x_; } + + /** + * \brief Returns the y-coordinate of this two-dimensional vector. + * \return The y-coordinate of the two-dimensional vector. + */ + inline float y() const { return y_; } + + /** + * \brief Computes the negation of this two-dimensional vector. + * \return The negation of this two-dimensional vector. + */ + inline Vector2 operator-() const + { + return Vector2(-x_, -y_); + } + + /** + * \brief Computes the dot product of this two-dimensional vector with + * the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * dot product should be computed. + * \return The dot product of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline float operator*(const Vector2 &vector) const + { + return x_ * vector.x() + y_ * vector.y(); + } + + /** + * \brief Computes the scalar multiplication of this + * two-dimensional vector with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \return The scalar multiplication of this two-dimensional vector + * with a specified scalar value. + */ + inline Vector2 operator*(float s) const + { + return Vector2(x_ * s, y_ * s); + } + + /** + * \brief Computes the scalar division of this two-dimensional vector + * with the specified scalar value. + * \param s The scalar value with which the scalar + * division should be computed. + * \return The scalar division of this two-dimensional vector with a + * specified scalar value. + */ + inline Vector2 operator/(float s) const + { + const float invS = 1.0f / s; + + return Vector2(x_ * invS, y_ * invS); + } + + /** + * \brief Computes the vector sum of this two-dimensional vector with + * the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector sum should be computed. + * \return The vector sum of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline Vector2 operator+(const Vector2 &vector) const + { + return Vector2(x_ + vector.x(), y_ + vector.y()); + } + + /** + * \brief Computes the vector difference of this two-dimensional + * vector with the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector difference should be computed. + * \return The vector difference of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline Vector2 operator-(const Vector2 &vector) const + { + return Vector2(x_ - vector.x(), y_ - vector.y()); + } + + /** + * \brief Tests this two-dimensional vector for equality with the + * specified two-dimensional vector. + * \param vector The two-dimensional vector with which to + * test for equality. + * \return True if the two-dimensional vectors are equal. + */ + inline bool operator==(const Vector2 &vector) const + { + return x_ == vector.x() && y_ == vector.y(); + } + + /** + * \brief Tests this two-dimensional vector for inequality with the + * specified two-dimensional vector. + * \param vector The two-dimensional vector with which to + * test for inequality. + * \return True if the two-dimensional vectors are not equal. + */ + inline bool operator!=(const Vector2 &vector) const + { + return x_ != vector.x() || y_ != vector.y(); + } + + /** + * \brief Sets the value of this two-dimensional vector to the scalar + * multiplication of itself with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator*=(float s) + { + x_ *= s; + y_ *= s; + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the scalar + * division of itself with the specified scalar value. + * \param s The scalar value with which the scalar + * division should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator/=(float s) + { + const float invS = 1.0f / s; + x_ *= invS; + y_ *= invS; + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the vector + * sum of itself with the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector sum should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator+=(const Vector2 &vector) + { + x_ += vector.x(); + y_ += vector.y(); + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the vector + * difference of itself with the specified two-dimensional + * vector. + * \param vector The two-dimensional vector with which the + * vector difference should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator-=(const Vector2 &vector) + { + x_ -= vector.x(); + y_ -= vector.y(); + + return *this; + } + + inline Vector2 &operator=(const Vector2 &vector) + { + x_ = vector.x(); + y_ = vector.y(); + + return *this; + } + + private: + float x_; + float y_; + }; + + /** + * \relates Vector2 + * \brief Computes the scalar multiplication of the specified + * two-dimensional vector with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \param vector The two-dimensional vector with which the scalar + * multiplication should be computed. + * \return The scalar multiplication of the two-dimensional vector with the + * scalar value. + */ + inline Vector2 operator*(float s, const Vector2 &vector) + { + return Vector2(s * vector.x(), s * vector.y()); + } + + /** + * \relates Vector2 + * \brief Inserts the specified two-dimensional vector into the specified + * output stream. + * \param os The output stream into which the two-dimensional + * vector should be inserted. + * \param vector The two-dimensional vector which to insert into + * the output stream. + * \return A reference to the output stream. + */ + inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector) + { + os << "(" << vector.x() << "," << vector.y() << ")"; + + return os; + } + + /** + * \relates Vector2 + * \brief Computes the length of a specified two-dimensional vector. + * \param vector The two-dimensional vector whose length is to be + * computed. + * \return The length of the two-dimensional vector. + */ + inline float abs(const Vector2 &vector) + { + return std::sqrt(vector * vector); + } + + /** + * \relates Vector2 + * \brief Computes the squared length of a specified two-dimensional + * vector. + * \param vector The two-dimensional vector whose squared length + * is to be computed. + * \return The squared length of the two-dimensional vector. + */ + inline float absSq(const Vector2 &vector) + { + return vector * vector; + } + + /** + * \relates Vector2 + * \brief Computes the determinant of a two-dimensional square matrix with + * rows consisting of the specified two-dimensional vectors. + * \param vector1 The top row of the two-dimensional square + * matrix. + * \param vector2 The bottom row of the two-dimensional square + * matrix. + * \return The determinant of the two-dimensional square matrix. + */ + inline float det(const Vector2 &vector1, const Vector2 &vector2) + { + return vector1.x() * vector2.y() - vector1.y() * vector2.x(); + } + + /** + * \relates Vector2 + * \brief Computes the normalization of the specified two-dimensional + * vector. + * \param vector The two-dimensional vector whose normalization + * is to be computed. + * \return The normalization of the two-dimensional vector. + */ + inline Vector2 normalize(const Vector2 &vector) + { + return vector / abs(vector); + } +} + +#endif /* RVO_VECTOR2_H_ */ diff --git a/thirdparty/rvo2/Agent.cpp b/thirdparty/rvo2/rvo2_3d/Agent3d.cpp similarity index 84% rename from thirdparty/rvo2/Agent.cpp rename to thirdparty/rvo2/rvo2_3d/Agent3d.cpp index b35eee9c12f..bddf226db18 100644 --- a/thirdparty/rvo2/Agent.cpp +++ b/thirdparty/rvo2/rvo2_3d/Agent3d.cpp @@ -30,15 +30,15 @@ * */ -#include "Agent.h" +#include "Agent3d.h" #include #include #include "Definitions.h" -#include "KdTree.h" +#include "KdTree3d.h" -namespace RVO { +namespace RVO3D { /** * \brief A sufficiently small positive number. */ @@ -47,7 +47,7 @@ namespace RVO { /** * \brief Defines a directed line. */ - class Line { + class Line3D { public: /** * \brief The direction of the directed line. @@ -71,7 +71,7 @@ namespace RVO { * \param result A reference to the result of the linear program. * \return True if successful. */ - bool linearProgram1(const std::vector &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result); + bool linearProgram1(const std::vector &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result); /** * \brief Solves a two-dimensional linear program on a specified plane subject to linear constraints defined by planes and a spherical constraint. @@ -105,42 +105,34 @@ namespace RVO { */ void linearProgram4(const std::vector &planes, size_t beginPlane, float radius, Vector3 &result); - Agent::Agent() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) { } + Agent3D::Agent3D() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { } - void Agent::computeNeighbors(KdTree *kdTree_) + void Agent3D::computeNeighbors(RVOSimulator3D *sim_) { agentNeighbors_.clear(); + if (maxNeighbors_ > 0) { - kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_); + sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_); } } - void Agent::computeNewVelocity(float timeStep) + void Agent3D::computeNewVelocity(RVOSimulator3D *sim_) { orcaPlanes_.clear(); + const float invTimeHorizon = 1.0f / timeHorizon_; /* Create agent ORCA planes. */ for (size_t i = 0; i < agentNeighbors_.size(); ++i) { - const Agent *const other = agentNeighbors_[i].second; + const Agent3D *const other = agentNeighbors_[i].second; - Vector3 relativePosition = other->position_ - position_; - Vector3 relativeVelocity = velocity_ - other->velocity_; - const float combinedRadius = radius_ + other->radius_; - - // This is a Godot feature that allow the agents to avoid the collision - // by moving only on the horizontal plane relative to the player velocity. - if (ignore_y_) { - // Skip if these are in two different heights -#define ABS(m_v) (((m_v) < 0) ? (-(m_v)) : (m_v)) - if (ABS(relativePosition[1]) > combinedRadius * 2) { - continue; - } - relativePosition[1] = 0; - relativeVelocity[1] = 0; - } + //const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f; + //const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod; + const Vector3 relativePosition = other->position_ - position_; + const Vector3 relativeVelocity = velocity_ - other->velocity_; const float distSq = absSq(relativePosition); + const float combinedRadius = radius_ + other->radius_; const float combinedRadiusSq = sqr(combinedRadius); Plane plane; @@ -168,17 +160,17 @@ namespace RVO { const float b = relativePosition * relativeVelocity; const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq); const float t = (b + std::sqrt(sqr(b) - a * c)) / a; - const Vector3 ww = relativeVelocity - t * relativePosition; - const float wwLength = abs(ww); - const Vector3 unitWW = ww / wwLength; + const Vector3 w = relativeVelocity - t * relativePosition; + const float wLength = abs(w); + const Vector3 unitW = w / wLength; - plane.normal = unitWW; - u = (combinedRadius * t - wwLength) * unitWW; + plane.normal = unitW; + u = (combinedRadius * t - wLength) * unitW; } } else { /* Collision. */ - const float invTimeStep = 1.0f / timeStep; + const float invTimeStep = 1.0f / sim_->timeStep_; const Vector3 w = relativeVelocity - invTimeStep * relativePosition; const float wLength = abs(w); const Vector3 unitW = w / wLength; @@ -196,40 +188,52 @@ namespace RVO { if (planeFail < orcaPlanes_.size()) { linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); } - - if (ignore_y_) { - // Not 100% necessary, but better to have. - newVelocity_[1] = prefVelocity_[1]; - } } - void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) + void Agent3D::insertAgentNeighbor(const Agent3D *agent, float &rangeSq) { - if (this != agent) { - const float distSq = absSq(position_ - agent->position_); + // no point processing same agent + if (this == agent) { + return; + } + // ignore other agent if layers/mask bitmasks have no matching bit + if ((avoidance_mask_ & agent->avoidance_layers_) == 0) { + return; + } - if (distSq < rangeSq) { - if (agentNeighbors_.size() < maxNeighbors_) { - agentNeighbors_.push_back(std::make_pair(distSq, agent)); - } + if (avoidance_priority_ > agent->avoidance_priority_) { + return; + } - size_t i = agentNeighbors_.size() - 1; + const float distSq = absSq(position_ - agent->position_); - while (i != 0 && distSq < agentNeighbors_[i - 1].first) { - agentNeighbors_[i] = agentNeighbors_[i - 1]; - --i; - } + if (distSq < rangeSq) { + if (agentNeighbors_.size() < maxNeighbors_) { + agentNeighbors_.push_back(std::make_pair(distSq, agent)); + } - agentNeighbors_[i] = std::make_pair(distSq, agent); + size_t i = agentNeighbors_.size() - 1; - if (agentNeighbors_.size() == maxNeighbors_) { - rangeSq = agentNeighbors_.back().first; - } + while (i != 0 && distSq < agentNeighbors_[i - 1].first) { + agentNeighbors_[i] = agentNeighbors_[i - 1]; + --i; + } + + agentNeighbors_[i] = std::make_pair(distSq, agent); + + if (agentNeighbors_.size() == maxNeighbors_) { + rangeSq = agentNeighbors_.back().first; } } } - bool linearProgram1(const std::vector &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) + void Agent3D::update(RVOSimulator3D *sim_) + { + velocity_ = newVelocity_; + position_ += velocity_ * sim_->timeStep_; + } + + bool linearProgram1(const std::vector &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) { const float dotProduct = line.point * line.direction; const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point); @@ -248,7 +252,7 @@ namespace RVO { const float denominator = line.direction * planes[i].normal; if (sqr(denominator) <= RVO3D_EPSILON) { - /* Lines line is (almost) parallel to plane i. */ + /* Lines3D line is (almost) parallel to plane i. */ if (numerator > 0.0f) { return false; } @@ -352,7 +356,7 @@ namespace RVO { return false; } - Line line; + Line3D line; line.direction = normalize(crossProduct); const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal); line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal; diff --git a/thirdparty/rvo2/Agent.h b/thirdparty/rvo2/rvo2_3d/Agent3d.h similarity index 63% rename from thirdparty/rvo2/Agent.h rename to thirdparty/rvo2/rvo2_3d/Agent3d.h index 45fbead2f5c..3e43646871a 100644 --- a/thirdparty/rvo2/Agent.h +++ b/thirdparty/rvo2/rvo2_3d/Agent3d.h @@ -8,7 +8,7 @@ * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * - * https://www.apache.org/licenses/LICENSE-2.0 + * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, @@ -27,7 +27,7 @@ * Chapel Hill, N.C. 27599-3175 * United States of America * - * + * */ /** @@ -41,77 +41,64 @@ #include #include +#include "RVOSimulator3d.h" #include "Vector3.h" -// Note: Slightly modified to work better in Godot. -// - The agent can be created by anyone. -// - The simulator pointer is removed. -// - The update function is removed. -// - The compute velocity function now need the timeStep. -// - Moved the `Plane` class here. -// - Added a new parameter `ignore_y_` in the `Agent`. This parameter is used to control a godot feature that allows to avoid collisions by moving on the horizontal plane. -namespace RVO { - /** - * \brief Defines a plane. - */ - class Plane { - public: - /** - * \brief A point on the plane. - */ - Vector3 point; - - /** - * \brief The normal to the plane. - */ - Vector3 normal; - }; - +namespace RVO3D { /** * \brief Defines an agent in the simulation. */ - class Agent { + class Agent3D { public: /** * \brief Constructs an agent instance. * \param sim The simulator instance. */ - explicit Agent(); + explicit Agent3D(); /** * \brief Computes the neighbors of this agent. */ - void computeNeighbors(class KdTree *kdTree_); + void computeNeighbors(RVOSimulator3D *sim_); /** * \brief Computes the new velocity of this agent. */ - void computeNewVelocity(float timeStep); + void computeNewVelocity(RVOSimulator3D *sim_); /** * \brief Inserts an agent neighbor into the set of neighbors of this agent. * \param agent A pointer to the agent to be inserted. * \param rangeSq The squared range around this agent. */ - void insertAgentNeighbor(const Agent *agent, float &rangeSq); + void insertAgentNeighbor(const Agent3D *agent, float &rangeSq); + + /** + * \brief Updates the three-dimensional position and three-dimensional velocity of this agent. + */ + void update(RVOSimulator3D *sim_); Vector3 newVelocity_; Vector3 position_; Vector3 prefVelocity_; Vector3 velocity_; + RVOSimulator3D *sim_; size_t id_; size_t maxNeighbors_; float maxSpeed_; float neighborDist_; float radius_; float timeHorizon_; - std::vector > agentNeighbors_; + float timeHorizonObst_; + std::vector > agentNeighbors_; std::vector orcaPlanes_; - /// This is a godot feature that allows the Agent to avoid collision by mooving - /// on the horizontal plane. - bool ignore_y_; + float height_ = 1.0; + uint32_t avoidance_layers_ = 1; + uint32_t avoidance_mask_ = 1; + float avoidance_priority_ = 1.0; - friend class KdTree; + friend class KdTree3D; + friend class RVOSimulator3D; }; } diff --git a/thirdparty/rvo2/Definitions.h b/thirdparty/rvo2/rvo2_3d/Definitions.h similarity index 98% rename from thirdparty/rvo2/Definitions.h rename to thirdparty/rvo2/rvo2_3d/Definitions.h index 707d3c897f6..34d1d06e0a0 100644 --- a/thirdparty/rvo2/Definitions.h +++ b/thirdparty/rvo2/rvo2_3d/Definitions.h @@ -38,7 +38,7 @@ #ifndef RVO3D_DEFINITIONS_H_ #define RVO3D_DEFINITIONS_H_ -namespace RVO { +namespace RVO3D { /** * \brief Computes the square of a float. * \param scalar The float to be squared. diff --git a/thirdparty/rvo2/KdTree.cpp b/thirdparty/rvo2/rvo2_3d/KdTree3d.cpp similarity index 92% rename from thirdparty/rvo2/KdTree.cpp rename to thirdparty/rvo2/rvo2_3d/KdTree3d.cpp index c857f299dfe..2534871db18 100644 --- a/thirdparty/rvo2/KdTree.cpp +++ b/thirdparty/rvo2/rvo2_3d/KdTree3d.cpp @@ -30,19 +30,20 @@ * */ -#include "KdTree.h" +#include "KdTree3d.h" #include -#include "Agent.h" +#include "Agent3d.h" #include "Definitions.h" +#include "RVOSimulator3d.h" -namespace RVO { +namespace RVO3D { const size_t RVO3D_MAX_LEAF_SIZE = 10; - KdTree::KdTree() { } + KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) { } - void KdTree::buildAgentTree(std::vector agents) + void KdTree3D::buildAgentTree(std::vector agents) { agents_.swap(agents); @@ -52,7 +53,7 @@ namespace RVO { } } - void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) + void KdTree3D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) { agentTree_[node].begin = begin; agentTree_[node].end = end; @@ -120,12 +121,12 @@ namespace RVO { } } - void KdTree::computeAgentNeighbors(Agent *agent, float rangeSq) const + void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const { queryAgentTreeRecursive(agent, rangeSq, 0); } - void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const + void KdTree3D::queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const { if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) { for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { diff --git a/thirdparty/rvo2/KdTree.h b/thirdparty/rvo2/rvo2_3d/KdTree3d.h similarity index 81% rename from thirdparty/rvo2/KdTree.h rename to thirdparty/rvo2/rvo2_3d/KdTree3d.h index 69d8920ce08..c018f98b233 100644 --- a/thirdparty/rvo2/KdTree.h +++ b/thirdparty/rvo2/rvo2_3d/KdTree3d.h @@ -41,22 +41,19 @@ #include "Vector3.h" -// Note: Slightly modified to work better with Godot. -// - Removed `sim_`. -// - KdTree things are public -namespace RVO { - class Agent; - class RVOSimulator; +namespace RVO3D { + class Agent3D; + class RVOSimulator3D; /** * \brief Defines kd-trees for agents in the simulation. */ - class KdTree { + class KdTree3D { public: /** * \brief Defines an agent kd-tree node. */ - class AgentTreeNode { + class AgentTreeNode3D { public: /** * \brief The beginning node number. @@ -93,12 +90,12 @@ namespace RVO { * \brief Constructs a kd-tree instance. * \param sim The simulator instance. */ - explicit KdTree(); + explicit KdTree3D(RVOSimulator3D *sim); /** * \brief Builds an agent kd-tree. */ - void buildAgentTree(std::vector agents); + void buildAgentTree(std::vector agents); void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); @@ -107,15 +104,16 @@ namespace RVO { * \param agent A pointer to the agent for which agent neighbors are to be computed. * \param rangeSq The squared range around the agent. */ - void computeAgentNeighbors(Agent *agent, float rangeSq) const; + void computeAgentNeighbors(Agent3D *agent, float rangeSq) const; - void queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const; + void queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const; - std::vector agents_; - std::vector agentTree_; + std::vector agents_; + std::vector agentTree_; + RVOSimulator3D *sim_; - friend class Agent; - friend class RVOSimulator; + friend class Agent3D; + friend class RVOSimulator3D; }; } diff --git a/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp new file mode 100644 index 00000000000..71e5aea9e2c --- /dev/null +++ b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp @@ -0,0 +1,274 @@ +/* + * RVOSimulator.cpp + * RVO2-3D Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "RVOSimulator3d.h" + +#ifdef _OPENMP +#include +#endif + +#include "Agent3d.h" +#include "KdTree3d.h" + +namespace RVO3D { + RVOSimulator3D::RVOSimulator3D() : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(0.0f) + { + kdTree_ = new KdTree3D(this); + } + + RVOSimulator3D::RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(timeStep) + { + kdTree_ = new KdTree3D(this); + defaultAgent_ = new Agent3D(); + + defaultAgent_->maxNeighbors_ = maxNeighbors; + defaultAgent_->maxSpeed_ = maxSpeed; + defaultAgent_->neighborDist_ = neighborDist; + defaultAgent_->radius_ = radius; + defaultAgent_->timeHorizon_ = timeHorizon; + defaultAgent_->velocity_ = velocity; + } + + RVOSimulator3D::~RVOSimulator3D() + { + if (defaultAgent_ != NULL) { + delete defaultAgent_; + } + + for (size_t i = 0; i < agents_.size(); ++i) { + delete agents_[i]; + } + + if (kdTree_ != NULL) { + delete kdTree_; + } + } + + size_t RVOSimulator3D::getAgentNumAgentNeighbors(size_t agentNo) const + { + return agents_[agentNo]->agentNeighbors_.size(); + } + + size_t RVOSimulator3D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const + { + return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; + } + + size_t RVOSimulator3D::getAgentNumORCAPlanes(size_t agentNo) const + { + return agents_[agentNo]->orcaPlanes_.size(); + } + + const Plane &RVOSimulator3D::getAgentORCAPlane(size_t agentNo, size_t planeNo) const + { + return agents_[agentNo]->orcaPlanes_[planeNo]; + } + + void RVOSimulator3D::removeAgent(size_t agentNo) + { + delete agents_[agentNo]; + agents_[agentNo] = agents_.back(); + agents_.pop_back(); + } + + size_t RVOSimulator3D::addAgent(const Vector3 &position) + { + if (defaultAgent_ == NULL) { + return RVO3D_ERROR; + } + + Agent3D *agent = new Agent3D(); + + agent->position_ = position; + agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; + agent->maxSpeed_ = defaultAgent_->maxSpeed_; + agent->neighborDist_ = defaultAgent_->neighborDist_; + agent->radius_ = defaultAgent_->radius_; + agent->timeHorizon_ = defaultAgent_->timeHorizon_; + agent->velocity_ = defaultAgent_->velocity_; + + agent->id_ = agents_.size(); + + agents_.push_back(agent); + + return agents_.size() - 1; + } + + size_t RVOSimulator3D::addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) + { + Agent3D *agent = new Agent3D(); + + agent->position_ = position; + agent->maxNeighbors_ = maxNeighbors; + agent->maxSpeed_ = maxSpeed; + agent->neighborDist_ = neighborDist; + agent->radius_ = radius; + agent->timeHorizon_ = timeHorizon; + agent->velocity_ = velocity; + + agent->id_ = agents_.size(); + + agents_.push_back(agent); + + return agents_.size() - 1; + } + + void RVOSimulator3D::doStep() + { + kdTree_->buildAgentTree(agents_); + + for (int i = 0; i < static_cast(agents_.size()); ++i) { + agents_[i]->computeNeighbors(this); + agents_[i]->computeNewVelocity(this); + } + + for (int i = 0; i < static_cast(agents_.size()); ++i) { + agents_[i]->update(this); + } + + globalTime_ += timeStep_; + } + + size_t RVOSimulator3D::getAgentMaxNeighbors(size_t agentNo) const + { + return agents_[agentNo]->maxNeighbors_; + } + + float RVOSimulator3D::getAgentMaxSpeed(size_t agentNo) const + { + return agents_[agentNo]->maxSpeed_; + } + + float RVOSimulator3D::getAgentNeighborDist(size_t agentNo) const + { + return agents_[agentNo]->neighborDist_; + } + + const Vector3 &RVOSimulator3D::getAgentPosition(size_t agentNo) const + { + return agents_[agentNo]->position_; + } + + const Vector3 &RVOSimulator3D::getAgentPrefVelocity(size_t agentNo) const + { + return agents_[agentNo]->prefVelocity_; + } + + float RVOSimulator3D::getAgentRadius(size_t agentNo) const + { + return agents_[agentNo]->radius_; + } + + float RVOSimulator3D::getAgentTimeHorizon(size_t agentNo) const + { + return agents_[agentNo]->timeHorizon_; + } + + const Vector3 &RVOSimulator3D::getAgentVelocity(size_t agentNo) const + { + return agents_[agentNo]->velocity_; + } + + float RVOSimulator3D::getGlobalTime() const + { + return globalTime_; + } + + size_t RVOSimulator3D::getNumAgents() const + { + return agents_.size(); + } + + float RVOSimulator3D::getTimeStep() const + { + return timeStep_; + } + + void RVOSimulator3D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) + { + if (defaultAgent_ == NULL) { + defaultAgent_ = new Agent3D(); + } + + defaultAgent_->maxNeighbors_ = maxNeighbors; + defaultAgent_->maxSpeed_ = maxSpeed; + defaultAgent_->neighborDist_ = neighborDist; + defaultAgent_->radius_ = radius; + defaultAgent_->timeHorizon_ = timeHorizon; + defaultAgent_->velocity_ = velocity; + } + + void RVOSimulator3D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) + { + agents_[agentNo]->maxNeighbors_ = maxNeighbors; + } + + void RVOSimulator3D::setAgentMaxSpeed(size_t agentNo, float maxSpeed) + { + agents_[agentNo]->maxSpeed_ = maxSpeed; + } + + void RVOSimulator3D::setAgentNeighborDist(size_t agentNo, float neighborDist) + { + agents_[agentNo]->neighborDist_ = neighborDist; + } + + void RVOSimulator3D::setAgentPosition(size_t agentNo, const Vector3 &position) + { + agents_[agentNo]->position_ = position; + } + + void RVOSimulator3D::setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity) + { + agents_[agentNo]->prefVelocity_ = prefVelocity; + } + + void RVOSimulator3D::setAgentRadius(size_t agentNo, float radius) + { + agents_[agentNo]->radius_ = radius; + } + + void RVOSimulator3D::setAgentTimeHorizon(size_t agentNo, float timeHorizon) + { + agents_[agentNo]->timeHorizon_ = timeHorizon; + } + + void RVOSimulator3D::setAgentVelocity(size_t agentNo, const Vector3 &velocity) + { + agents_[agentNo]->velocity_ = velocity; + } + + void RVOSimulator3D::setTimeStep(float timeStep) + { + timeStep_ = timeStep; + } +} diff --git a/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h new file mode 100644 index 00000000000..4ea093d74ce --- /dev/null +++ b/thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h @@ -0,0 +1,324 @@ +/* + * RVOSimulator.h + * RVO2-3D Library + * + * Copyright 2008 University of North Carolina at Chapel Hill + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +/** + * \file RVOSimulator.h + * \brief Contains the RVOSimulator class. + */ +#ifndef RVO3D_RVO_SIMULATOR_H_ +#define RVO3D_RVO_SIMULATOR_H_ + +#include +#include +#include + +#include "Vector3.h" + +namespace RVO3D { + class Agent3D; + class KdTree3D; + + /** + * \brief Error value. + * + * A value equal to the largest unsigned integer, which is returned in case of an error by functions in RVO3D::RVOSimulator. + */ + const size_t RVO3D_ERROR = std::numeric_limits::max(); + + /** + * \brief Defines a plane. + */ + class Plane { + public: + /** + * \brief A point on the plane. + */ + Vector3 point; + + /** + * \brief The normal to the plane. + */ + Vector3 normal; + }; + + /** + * \brief Defines the simulation. + * + * The main class of the library that contains all simulation functionality. + */ + class RVOSimulator3D { + public: + /** + * \brief Constructs a simulator instance. + */ + RVOSimulator3D(); + + /** + * \brief Constructs a simulator instance and sets the default properties for any new agent that is added. + * \param timeStep The time step of the simulation. Must be positive. + * \param neighborDist The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative. + * \param maxNeighbors The default maximum number of other agents a new 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. + * \param timeHorizon The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive. + * \param radius The default radius of a new agent. Must be non-negative. + * \param maxSpeed The default maximum speed of a new agent. Must be non-negative. + * \param velocity The default initial three-dimensional linear velocity of a new agent (optional). + */ + RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3()); + + /** + * \brief Destroys this simulator instance. + */ + ~RVOSimulator3D(); + + /** + * \brief Adds a new agent with default properties to the simulation. + * \param position The three-dimensional starting position of this agent. + * \return The number of the agent, or RVO3D::RVO3D_ERROR when the agent defaults have not been set. + */ + size_t addAgent(const Vector3 &position); + + /** + * \brief Adds a new agent to the simulation. + * \param position The three-dimensional starting position of this agent. + * \param neighborDist The maximum distance (center point to center point) to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative. + * \param maxNeighbors The maximum number of other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe. + * \param timeHorizon The minimum amount of time for which this agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive. + * \param radius The radius of this agent. Must be non-negative. + * \param maxSpeed The maximum speed of this agent. Must be non-negative. + * \param velocity The initial three-dimensional linear velocity of this agent (optional). + * \return The number of the agent. + */ + size_t addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3()); + + /** + * \brief Lets the simulator perform a simulation step and updates the three-dimensional position and three-dimensional velocity of each agent. + */ + void doStep(); + + /** + * \brief Returns the specified agent neighbor of the specified agent. + * \param agentNo The number of the agent whose agent neighbor is to be retrieved. + * \param neighborNo The number of the agent neighbor to be retrieved. + * \return The number of the neighboring agent. + */ + size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const; + + /** + * \brief Returns the maximum neighbor count of a specified agent. + * \param agentNo The number of the agent whose maximum neighbor count is to be retrieved. + * \return The present maximum neighbor count of the agent. + */ + size_t getAgentMaxNeighbors(size_t agentNo) const; + + /** + * \brief Returns the maximum speed of a specified agent. + * \param agentNo The number of the agent whose maximum speed is to be retrieved. + * \return The present maximum speed of the agent. + */ + float getAgentMaxSpeed(size_t agentNo) const; + + /** + * \brief Returns the maximum neighbor distance of a specified agent. + * \param agentNo The number of the agent whose maximum neighbor distance is to be retrieved. + * \return The present maximum neighbor distance of the agent. + */ + float getAgentNeighborDist(size_t agentNo) const; + + /** + * \brief Returns the count of agent neighbors taken into account to compute the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of agent neighbors is to be retrieved. + * \return The count of agent neighbors taken into account to compute the current velocity for the specified agent. + */ + size_t getAgentNumAgentNeighbors(size_t agentNo) const; + + /** + * \brief Returns the count of ORCA constraints used to compute the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of ORCA constraints is to be retrieved. + * \return The count of ORCA constraints used to compute the current velocity for the specified agent. + */ + size_t getAgentNumORCAPlanes(size_t agentNo) const; + + /** + * \brief Returns the specified ORCA constraint of the specified agent. + * \param agentNo The number of the agent whose ORCA constraint is to be retrieved. + * \param planeNo The number of the ORCA constraint to be retrieved. + * \return A plane representing the specified ORCA constraint. + * \note The halfspace to which the normal of the plane points is the region of permissible velocities with respect to the specified ORCA constraint. + */ + const Plane &getAgentORCAPlane(size_t agentNo, size_t planeNo) const; + + /** + * \brief Returns the three-dimensional position of a specified agent. + * \param agentNo The number of the agent whose three-dimensional position is to be retrieved. + * \return The present three-dimensional position of the (center of the) agent. + */ + const Vector3 &getAgentPosition(size_t agentNo) const; + + /** + * \brief Returns the three-dimensional preferred velocity of a specified agent. + * \param agentNo The number of the agent whose three-dimensional preferred velocity is to be retrieved. + * \return The present three-dimensional preferred velocity of the agent. + */ + const Vector3 &getAgentPrefVelocity(size_t agentNo) const; + + /** + * \brief Returns the radius of a specified agent. + * \param agentNo The number of the agent whose radius is to be retrieved. + * \return The present radius of the agent. + */ + float getAgentRadius(size_t agentNo) const; + + /** + * \brief Returns the time horizon of a specified agent. + * \param agentNo The number of the agent whose time horizon is to be retrieved. + * \return The present time horizon of the agent. + */ + float getAgentTimeHorizon(size_t agentNo) const; + + /** + * \brief Returns the three-dimensional linear velocity of a specified agent. + * \param agentNo The number of the agent whose three-dimensional linear velocity is to be retrieved. + * \return The present three-dimensional linear velocity of the agent. + */ + const Vector3 &getAgentVelocity(size_t agentNo) const; + + /** + * \brief Returns the global time of the simulation. + * \return The present global time of the simulation (zero initially). + */ + float getGlobalTime() const; + + /** + * \brief Returns the count of agents in the simulation. + * \return The count of agents in the simulation. + */ + size_t getNumAgents() const; + + /** + * \brief Returns the time step of the simulation. + * \return The present time step of the simulation. + */ + float getTimeStep() const; + + /** + * \brief Removes an agent from the simulation. + * \param agentNo The number of the agent that is to be removed. + * \note After the removal of the agent, the agent that previously had number getNumAgents() - 1 will now have number agentNo. + */ + void removeAgent(size_t agentNo); + + /** + * \brief Sets the default properties for any new agent that is added. + * \param neighborDist The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative. + * \param maxNeighbors The default maximum number of other agents a new 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. + * \param timeHorizon The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive. + * \param radius The default radius of a new agent. Must be non-negative. + * \param maxSpeed The default maximum speed of a new agent. Must be non-negative. + * \param velocity The default initial three-dimensional linear velocity of a new agent (optional). + */ + void setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3()); + + /** + * \brief Sets the maximum neighbor count of a specified agent. + * \param agentNo The number of the agent whose maximum neighbor count is to be modified. + * \param maxNeighbors The replacement maximum neighbor count. + */ + void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors); + + /** + * \brief Sets the maximum speed of a specified agent. + * \param agentNo The number of the agent whose maximum speed is to be modified. + * \param maxSpeed The replacement maximum speed. Must be non-negative. + */ + void setAgentMaxSpeed(size_t agentNo, float maxSpeed); + + /** + * \brief Sets the maximum neighbor distance of a specified agent. + * \param agentNo The number of the agent whose maximum neighbor distance is to be modified. + * \param neighborDist The replacement maximum neighbor distance. Must be non-negative. + */ + void setAgentNeighborDist(size_t agentNo, float neighborDist); + + /** + * \brief Sets the three-dimensional position of a specified agent. + * \param agentNo The number of the agent whose three-dimensional position is to be modified. + * \param position The replacement of the three-dimensional position. + */ + void setAgentPosition(size_t agentNo, const Vector3 &position); + + /** + * \brief Sets the three-dimensional preferred velocity of a specified agent. + * \param agentNo The number of the agent whose three-dimensional preferred velocity is to be modified. + * \param prefVelocity The replacement of the three-dimensional preferred velocity. + */ + void setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity); + + /** + * \brief Sets the radius of a specified agent. + * \param agentNo The number of the agent whose radius is to be modified. + * \param radius The replacement radius. Must be non-negative. + */ + void setAgentRadius(size_t agentNo, float radius); + + /** + * \brief Sets the time horizon of a specified agent with respect to other agents. + * \param agentNo The number of the agent whose time horizon is to be modified. + * \param timeHorizon The replacement time horizon with respect to other agents. Must be positive. + */ + void setAgentTimeHorizon(size_t agentNo, float timeHorizon); + + /** + * \brief Sets the three-dimensional linear velocity of a specified agent. + * \param agentNo The number of the agent whose three-dimensional linear velocity is to be modified. + * \param velocity The replacement three-dimensional linear velocity. + */ + void setAgentVelocity(size_t agentNo, const Vector3 &velocity); + + /** + * \brief Sets the time step of the simulation. + * \param timeStep The time step of the simulation. Must be positive. + */ + void setTimeStep(float timeStep); + + public: + Agent3D *defaultAgent_; + KdTree3D *kdTree_; + float globalTime_; + float timeStep_; + std::vector agents_; + + friend class Agent3D; + friend class KdTree3D; + }; +} + +#endif diff --git a/thirdparty/rvo2/Vector3.h b/thirdparty/rvo2/rvo2_3d/Vector3.h similarity index 92% rename from thirdparty/rvo2/Vector3.h rename to thirdparty/rvo2/rvo2_3d/Vector3.h index f44e311f292..6fa4bb074c4 100644 --- a/thirdparty/rvo2/Vector3.h +++ b/thirdparty/rvo2/rvo2_3d/Vector3.h @@ -41,13 +41,11 @@ #include #include -#define RVO3D_EXPORT - -namespace RVO { +namespace RVO3D { /** * \brief Defines a three-dimensional vector. */ - class RVO3D_EXPORT Vector3 { + class Vector3 { public: /** * \brief Constructs and initializes a three-dimensional vector instance to zero. @@ -59,6 +57,17 @@ namespace RVO { val_[2] = 0.0f; } + /** + * \brief Constructs and initializes a three-dimensional vector from the specified three-dimensional vector. + * \param vector The three-dimensional vector containing the xyz-coordinates. + */ + inline Vector3(const Vector3 &vector) + { + val_[0] = vector[0]; + val_[1] = vector[1]; + val_[2] = vector[2]; + } + /** * \brief Constructs and initializes a three-dimensional vector from the specified three-element array. * \param val The three-element array containing the xyz-coordinates. @@ -255,6 +264,15 @@ namespace RVO { return *this; } + inline Vector3 &operator=(const Vector3 &vector) + { + val_[0] = vector[0]; + val_[1] = vector[1]; + val_[2] = vector[2]; + + return *this; + } + private: float val_[3]; }; @@ -267,7 +285,7 @@ namespace RVO { * \param vector The three-dimensional vector with which the scalar multiplication should be computed. * \return The scalar multiplication of the three-dimensional vector with the scalar value. */ - RVO3D_EXPORT inline Vector3 operator*(float scalar, const Vector3 &vector) + inline Vector3 operator*(float scalar, const Vector3 &vector) { return Vector3(scalar * vector[0], scalar * vector[1], scalar * vector[2]); } @@ -279,7 +297,7 @@ namespace RVO { * \param vector2 The second vector with which the cross product should be computed. * \return The cross product of the two specified vectors. */ - RVO3D_EXPORT inline Vector3 cross(const Vector3 &vector1, const Vector3 &vector2) + inline Vector3 cross(const Vector3 &vector1, const Vector3 &vector2) { return Vector3(vector1[1] * vector2[2] - vector1[2] * vector2[1], vector1[2] * vector2[0] - vector1[0] * vector2[2], vector1[0] * vector2[1] - vector1[1] * vector2[0]); } @@ -291,7 +309,7 @@ namespace RVO { * \param vector The three-dimensional vector which to insert into the output stream. * \return A reference to the output stream. */ - RVO3D_EXPORT inline std::ostream &operator<<(std::ostream &os, const Vector3 &vector) + inline std::ostream &operator<<(std::ostream &os, const Vector3 &vector) { os << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")"; @@ -304,7 +322,7 @@ namespace RVO { * \param vector The three-dimensional vector whose length is to be computed. * \return The length of the three-dimensional vector. */ - RVO3D_EXPORT inline float abs(const Vector3 &vector) + inline float abs(const Vector3 &vector) { return std::sqrt(vector * vector); } @@ -315,7 +333,7 @@ namespace RVO { * \param vector The three-dimensional vector whose squared length is to be computed. * \return The squared length of the three-dimensional vector. */ - RVO3D_EXPORT inline float absSq(const Vector3 &vector) + inline float absSq(const Vector3 &vector) { return vector * vector; } @@ -326,7 +344,7 @@ namespace RVO { * \param vector The three-dimensional vector whose normalization is to be computed. * \return The normalization of the three-dimensional vector. */ - RVO3D_EXPORT inline Vector3 normalize(const Vector3 &vector) + inline Vector3 normalize(const Vector3 &vector) { return vector / abs(vector); }