Rework Navigation Avoidance

Rework Navigation Avoidance.

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))

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);

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,"

doc/classes/@GlobalScope.xml

@@ -2695,36 +2695,39 @@
 		<constant name="PROPERTY_HINT_LAYERS_3D_NAVIGATION" value="12" enum="PropertyHint">
 			Hints that an [int] property is a bitmask using the optionally named 3D navigation layers.
 		</constant>
-		<constant name="PROPERTY_HINT_FILE" value="13" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_LAYERS_AVOIDANCE" value="13" enum="PropertyHint">
+			Hints that an integer property is a bitmask using the optionally named avoidance layers.
+		</constant>
+		<constant name="PROPERTY_HINT_FILE" value="14" enum="PropertyHint">
 			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].
 		</constant>
-		<constant name="PROPERTY_HINT_DIR" value="14" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_DIR" value="15" enum="PropertyHint">
 			Hints that a [String] property is a path to a directory. Editing it will show a file dialog for picking the path.
 		</constant>
-		<constant name="PROPERTY_HINT_GLOBAL_FILE" value="15" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_GLOBAL_FILE" value="16" enum="PropertyHint">
 			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].
 		</constant>
-		<constant name="PROPERTY_HINT_GLOBAL_DIR" value="16" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_GLOBAL_DIR" value="17" enum="PropertyHint">
 			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.
 		</constant>
-		<constant name="PROPERTY_HINT_RESOURCE_TYPE" value="17" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_RESOURCE_TYPE" value="18" enum="PropertyHint">
 			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.
 		</constant>
-		<constant name="PROPERTY_HINT_MULTILINE_TEXT" value="18" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_MULTILINE_TEXT" value="19" enum="PropertyHint">
 			Hints that a [String] property is text with line breaks. Editing it will show a text input field where line breaks can be typed.
 		</constant>
-		<constant name="PROPERTY_HINT_EXPRESSION" value="19" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_EXPRESSION" value="20" enum="PropertyHint">
 			Hints that a [String] property is an [Expression].
 		</constant>
-		<constant name="PROPERTY_HINT_PLACEHOLDER_TEXT" value="20" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_PLACEHOLDER_TEXT" value="21" enum="PropertyHint">
 			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.
 		</constant>
-		<constant name="PROPERTY_HINT_COLOR_NO_ALPHA" value="21" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_COLOR_NO_ALPHA" value="22" enum="PropertyHint">
 			Hints that a [Color] property should be edited without affecting its transparency ([member Color.a] is not editable).
 		</constant>
-		<constant name="PROPERTY_HINT_OBJECT_ID" value="22" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_OBJECT_ID" value="23" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_TYPE_STRING" value="23" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_TYPE_STRING" value="24" enum="PropertyHint">
 			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.
 		</constant>
-		<constant name="PROPERTY_HINT_NODE_PATH_TO_EDITED_NODE" value="24" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_NODE_PATH_TO_EDITED_NODE" value="25" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_OBJECT_TOO_BIG" value="25" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_OBJECT_TOO_BIG" value="26" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_NODE_PATH_VALID_TYPES" value="26" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_NODE_PATH_VALID_TYPES" value="27" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_SAVE_FILE" value="27" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_SAVE_FILE" value="28" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_GLOBAL_SAVE_FILE" value="28" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_GLOBAL_SAVE_FILE" value="29" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_INT_IS_OBJECTID" value="29" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_INT_IS_OBJECTID" value="30" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_INT_IS_POINTER" value="30" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_INT_IS_POINTER" value="31" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_ARRAY_TYPE" value="31" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_ARRAY_TYPE" value="32" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_LOCALE_ID" value="32" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_LOCALE_ID" value="33" enum="PropertyHint">
 			Hints that a string property is a locale code. Editing it will show a locale dialog for picking language and country.
 		</constant>
-		<constant name="PROPERTY_HINT_LOCALIZABLE_STRING" value="33" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_LOCALIZABLE_STRING" value="34" enum="PropertyHint">
 			Hints that a dictionary property is string translation map. Dictionary keys are locale codes and, values are translated strings.
 		</constant>
-		<constant name="PROPERTY_HINT_NODE_TYPE" value="34" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_NODE_TYPE" value="35" enum="PropertyHint">
 		</constant>
-		<constant name="PROPERTY_HINT_HIDE_QUATERNION_EDIT" value="35" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_HIDE_QUATERNION_EDIT" value="36" enum="PropertyHint">
 			Hints that a quaternion property should disable the temporary euler editor.
 		</constant>
-		<constant name="PROPERTY_HINT_PASSWORD" value="36" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_PASSWORD" value="37" enum="PropertyHint">
 			Hints that a string property is a password, and every character is replaced with the secret character.
 		</constant>
-		<constant name="PROPERTY_HINT_MAX" value="37" enum="PropertyHint">
+		<constant name="PROPERTY_HINT_MAX" value="38" enum="PropertyHint">
 			Represents the size of the [enum PropertyHint] enum.
 		</constant>
 		<constant name="PROPERTY_USAGE_NONE" value="0" enum="PropertyUsageFlags" is_bitfield="true">

doc/classes/NavigationAgent2D.xml

@@ -5,7 +5,7 @@
 	</brief_description>
 	<description>
 		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.
 	</description>
 	<tutorials>
 		<link title="Using NavigationAgents">$DOCS_URL/tutorials/navigation/navigation_using_navigationagents.html</link>
@@ -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.
 			</description>
 		</method>
+		<method name="get_avoidance_layer_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="layer_number" type="int" />
+			<description>
+				Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32.
+			</description>
+		</method>
+		<method name="get_avoidance_mask_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="mask_number" type="int" />
+			<description>
+				Returns whether or not the specified mask of the [member avoidance_mask] bitmask is enabled, given a [param mask_number] between 1 and 32.
+			</description>
+		</method>
 		<method name="get_current_navigation_path" qualifiers="const">
 			<return type="PackedVector2Array" />
 			<description>
@@ -38,7 +52,7 @@
 		<method name="get_final_position">
 			<return type="Vector2" />
 			<description>
-				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.
 			</description>
 		</method>
 		<method name="get_navigation_layer_value" qualifiers="const">
@@ -75,7 +89,7 @@
 		<method name="is_target_reachable">
 			<return type="bool" />
 			<description>
-				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].
 			</description>
 		</method>
 		<method name="is_target_reached" qualifiers="const">
@@ -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].
 			</description>
 		</method>
+		<method name="set_avoidance_layer_value">
+			<return type="void" />
+			<param index="0" name="layer_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
+			</description>
+		</method>
+		<method name="set_avoidance_mask_value">
+			<return type="void" />
+			<param index="0" name="mask_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
+			</description>
+		</method>
 		<method name="set_navigation_layer_value">
 			<return type="void" />
 			<param index="0" name="layer_number" type="int" />
@@ -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.
 			</description>
 		</method>
-		<method name="set_velocity">
+		<method name="set_velocity_forced">
 			<return type="void" />
 			<param index="0" name="velocity" type="Vector2" />
 			<description>
-				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.
 			</description>
 		</method>
 	</methods>
 	<members>
 		<member name="avoidance_enabled" type="bool" setter="set_avoidance_enabled" getter="get_avoidance_enabled" default="false">
-			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.
+		</member>
+		<member name="avoidance_layers" type="int" setter="set_avoidance_layers" getter="get_avoidance_layers" default="1">
+			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.
+		</member>
+		<member name="avoidance_mask" type="int" setter="set_avoidance_mask" getter="get_avoidance_mask" default="1">
+			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].
+		</member>
+		<member name="avoidance_priority" type="float" setter="set_avoidance_priority" getter="get_avoidance_priority" default="1.0">
+			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.
 		</member>
 		<member name="debug_enabled" type="bool" setter="set_debug_enabled" getter="get_debug_enabled" default="false">
 			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.
 		</member>
 		<member name="target_position" type="Vector2" setter="set_target_position" getter="get_target_position" default="Vector2(0, 0)">
-			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.
+		</member>
+		<member name="time_horizon_agents" type="float" setter="set_time_horizon_agents" getter="get_time_horizon_agents" default="1.0">
+			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.
+		</member>
+		<member name="time_horizon_obstacles" type="float" setter="set_time_horizon_obstacles" getter="get_time_horizon_obstacles" default="0.0">
+			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.
 		</member>
-		<member name="time_horizon" type="float" setter="set_time_horizon" getter="get_time_horizon" default="1.0">
-			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.
+		<member name="velocity" type="Vector2" setter="set_velocity" getter="get_velocity" default="Vector2(0, 0)">
+			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.
 		</member>
 	</members>
 	<signals>
@@ -199,7 +244,7 @@
 		<signal name="velocity_computed">
 			<param index="0" name="safe_velocity" type="Vector2" />
 			<description>
-				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.
 			</description>
 		</signal>
 		<signal name="waypoint_reached">

doc/classes/NavigationAgent3D.xml

@@ -5,7 +5,7 @@
 	</brief_description>
 	<description>
 		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.
 	</description>
 	<tutorials>
 		<link title="Using NavigationAgents">$DOCS_URL/tutorials/navigation/navigation_using_navigationagents.html</link>
@@ -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.
 			</description>
 		</method>
+		<method name="get_avoidance_layer_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="layer_number" type="int" />
+			<description>
+				Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32.
+			</description>
+		</method>
+		<method name="get_avoidance_mask_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="mask_number" type="int" />
+			<description>
+				Returns whether or not the specified mask of the [member avoidance_mask] bitmask is enabled, given a [param mask_number] between 1 and 32.
+			</description>
+		</method>
 		<method name="get_current_navigation_path" qualifiers="const">
 			<return type="PackedVector3Array" />
 			<description>
@@ -38,7 +52,7 @@
 		<method name="get_final_position">
 			<return type="Vector3" />
 			<description>
-				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.
 			</description>
 		</method>
 		<method name="get_navigation_layer_value" qualifiers="const">
@@ -75,7 +89,7 @@
 		<method name="is_target_reachable">
 			<return type="bool" />
 			<description>
-				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].
 			</description>
 		</method>
 		<method name="is_target_reached" qualifiers="const">
@@ -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].
 			</description>
 		</method>
+		<method name="set_avoidance_layer_value">
+			<return type="void" />
+			<param index="0" name="layer_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
+			</description>
+		</method>
+		<method name="set_avoidance_mask_value">
+			<return type="void" />
+			<param index="0" name="mask_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
+			</description>
+		</method>
 		<method name="set_navigation_layer_value">
 			<return type="void" />
 			<param index="0" name="layer_number" type="int" />
@@ -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.
 			</description>
 		</method>
-		<method name="set_velocity">
+		<method name="set_velocity_forced">
 			<return type="void" />
 			<param index="0" name="velocity" type="Vector3" />
 			<description>
-				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.
 			</description>
 		</method>
 	</methods>
 	<members>
-		<member name="agent_height_offset" type="float" setter="set_agent_height_offset" getter="get_agent_height_offset" default="0.0">
-			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.
-		</member>
 		<member name="avoidance_enabled" type="bool" setter="set_avoidance_enabled" getter="get_avoidance_enabled" default="false">
-			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.
+		</member>
+		<member name="avoidance_layers" type="int" setter="set_avoidance_layers" getter="get_avoidance_layers" default="1">
+			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.
+		</member>
+		<member name="avoidance_mask" type="int" setter="set_avoidance_mask" getter="get_avoidance_mask" default="1">
+			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].
+		</member>
+		<member name="avoidance_priority" type="float" setter="set_avoidance_priority" getter="get_avoidance_priority" default="1.0">
+			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.
 		</member>
 		<member name="debug_enabled" type="bool" setter="set_debug_enabled" getter="get_debug_enabled" default="false">
 			If [code]true[/code] shows debug visuals for this agent.
@@ -126,8 +162,8 @@
 		<member name="debug_use_custom" type="bool" setter="set_debug_use_custom" getter="get_debug_use_custom" default="false">
 			If [code]true[/code] uses the defined [member debug_path_custom_color] for this agent instead of global color.
 		</member>
-		<member name="ignore_y" type="bool" setter="set_ignore_y" getter="get_ignore_y" default="true">
-			Ignores collisions on the Y axis. Must be true to move on a horizontal plane.
+		<member name="height" type="float" setter="set_height" getter="get_height" default="1.0">
+			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.
 		</member>
 		<member name="max_neighbors" type="int" setter="set_max_neighbors" getter="get_max_neighbors" default="10">
 			The maximum number of neighbors for the agent to consider.
@@ -144,6 +180,9 @@
 		<member name="path_desired_distance" type="float" setter="set_path_desired_distance" getter="get_path_desired_distance" default="1.0">
 			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.
 		</member>
+		<member name="path_height_offset" type="float" setter="set_path_height_offset" getter="get_path_height_offset" default="0.0">
+			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.
+		</member>
 		<member name="path_max_distance" type="float" setter="set_path_max_distance" getter="get_path_max_distance" default="5.0">
 			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.
 		</member>
@@ -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.
 		</member>
 		<member name="target_position" type="Vector3" setter="set_target_position" getter="get_target_position" default="Vector3(0, 0, 0)">
-			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.
+		</member>
+		<member name="time_horizon_agents" type="float" setter="set_time_horizon_agents" getter="get_time_horizon_agents" default="1.0">
+			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.
+		</member>
+		<member name="time_horizon_obstacles" type="float" setter="set_time_horizon_obstacles" getter="get_time_horizon_obstacles" default="0.0">
+			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.
+		</member>
+		<member name="use_3d_avoidance" type="bool" setter="set_use_3d_avoidance" getter="get_use_3d_avoidance" default="false">
+			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.
 		</member>
-		<member name="time_horizon" type="float" setter="set_time_horizon" getter="get_time_horizon" default="1.0">
-			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.
+		<member name="velocity" type="Vector3" setter="set_velocity" getter="get_velocity" default="Vector3(0, 0, 0)">
+			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.
 		</member>
 	</members>
 	<signals>
@@ -202,7 +251,7 @@
 		<signal name="velocity_computed">
 			<param index="0" name="safe_velocity" type="Vector3" />
 			<description>
-				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.
 			</description>
 		</signal>
 		<signal name="waypoint_reached">

doc/classes/NavigationObstacle2D.xml

@@ -1,42 +1,71 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="NavigationObstacle2D" inherits="Node" is_experimental="true" version="4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+<class name="NavigationObstacle2D" inherits="Node2D" is_experimental="true" version="4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
 	<brief_description>
-		2D Obstacle used in navigation for collision avoidance.
+		2D Obstacle used in navigation to constrain avoidance controlled agents outside or inside an area.
 	</brief_description>
 	<description>
-		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.
 	</description>
 	<tutorials>
 		<link title="Using NavigationObstacles">$DOCS_URL/tutorials/navigation/navigation_using_navigationobstacles.html</link>
 	</tutorials>
 	<methods>
+		<method name="get_agent_rid" qualifiers="const">
+			<return type="RID" />
+			<description>
+				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].
+			</description>
+		</method>
+		<method name="get_avoidance_layer_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="layer_number" type="int" />
+			<description>
+				Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32.
+			</description>
+		</method>
 		<method name="get_navigation_map" qualifiers="const">
 			<return type="RID" />
 			<description>
-				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.
 			</description>
 		</method>
-		<method name="get_rid" qualifiers="const">
+		<method name="get_obstacle_rid" qualifiers="const">
 			<return type="RID" />
 			<description>
-				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].
+			</description>
+		</method>
+		<method name="set_avoidance_layer_value">
+			<return type="void" />
+			<param index="0" name="layer_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
 			</description>
 		</method>
 		<method name="set_navigation_map">
 			<return type="void" />
 			<param index="0" name="navigation_map" type="RID" />
 			<description>
-				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.
 			</description>
 		</method>
 	</methods>
 	<members>
-		<member name="estimate_radius" type="bool" setter="set_estimate_radius" getter="is_radius_estimated" default="true">
-			Enables radius estimation algorithm which uses parent's collision shapes to determine the obstacle radius.
+		<member name="avoidance_layers" type="int" setter="set_avoidance_layers" getter="get_avoidance_layers" default="1">
+			A bitfield determining the avoidance layers for this obstacle. Agent's with a matching bit on the their avoidance mask will avoid this obstacle.
+		</member>
+		<member name="radius" type="float" setter="set_radius" getter="get_radius" default="0.0">
+			Sets the avoidance radius for the obstacle.
+		</member>
+		<member name="velocity" type="Vector2" setter="set_velocity" getter="get_velocity" default="Vector2(0, 0)">
+			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.
 		</member>
-		<member name="radius" type="float" setter="set_radius" getter="get_radius" default="1.0">
-			The radius of the agent. Used only if [member estimate_radius] is set to false.
+		<member name="vertices" type="PackedVector2Array" setter="set_vertices" getter="get_vertices" default="PackedVector2Array()">
+			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.
 		</member>
 	</members>
 </class>

doc/classes/NavigationObstacle3D.xml

@@ -1,42 +1,78 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<class name="NavigationObstacle3D" inherits="Node" is_experimental="true" version="4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
+<class name="NavigationObstacle3D" inherits="Node3D" is_experimental="true" version="4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="../class.xsd">
 	<brief_description>
-		3D Obstacle used in navigation for collision avoidance.
+		3D Obstacle used in navigation to constrain avoidance controlled agents outside or inside an area.
 	</brief_description>
 	<description>
-		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.
 	</description>
 	<tutorials>
 		<link title="Using NavigationObstacles">$DOCS_URL/tutorials/navigation/navigation_using_navigationobstacles.html</link>
 	</tutorials>
 	<methods>
+		<method name="get_agent_rid" qualifiers="const">
+			<return type="RID" />
+			<description>
+				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].
+			</description>
+		</method>
+		<method name="get_avoidance_layer_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="layer_number" type="int" />
+			<description>
+				Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32.
+			</description>
+		</method>
 		<method name="get_navigation_map" qualifiers="const">
 			<return type="RID" />
 			<description>
-				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.
 			</description>
 		</method>
-		<method name="get_rid" qualifiers="const">
+		<method name="get_obstacle_rid" qualifiers="const">
 			<return type="RID" />
 			<description>
-				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].
+			</description>
+		</method>
+		<method name="set_avoidance_layer_value">
+			<return type="void" />
+			<param index="0" name="layer_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
 			</description>
 		</method>
 		<method name="set_navigation_map">
 			<return type="void" />
 			<param index="0" name="navigation_map" type="RID" />
 			<description>
-				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.
 			</description>
 		</method>
 	</methods>
 	<members>
-		<member name="estimate_radius" type="bool" setter="set_estimate_radius" getter="is_radius_estimated" default="true">
-			Enables radius estimation algorithm which uses parent's collision shapes to determine the obstacle radius.
+		<member name="avoidance_layers" type="int" setter="set_avoidance_layers" getter="get_avoidance_layers" default="1">
+			A bitfield determining the avoidance layers for this obstacle. Agent's with a matching bit on the their avoidance mask will avoid this obstacle.
+		</member>
+		<member name="height" type="float" setter="set_height" getter="get_height" default="1.0">
+			Sets the obstacle height used in 2D avoidance. 2D avoidance using agent's ignore obstacles that are below or above them.
+		</member>
+		<member name="radius" type="float" setter="set_radius" getter="get_radius" default="0.0">
+			Sets the avoidance radius for the obstacle.
+		</member>
+		<member name="use_3d_avoidance" type="bool" setter="set_use_3d_avoidance" getter="get_use_3d_avoidance" default="false">
+			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].
+		</member>
+		<member name="velocity" type="Vector3" setter="set_velocity" getter="get_velocity" default="Vector3(0, 0, 0)">
+			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.
 		</member>
-		<member name="radius" type="float" setter="set_radius" getter="get_radius" default="1.0">
-			The radius of the agent. Used only if [member estimate_radius] is set to false.
+		<member name="vertices" type="PackedVector3Array" setter="set_vertices" getter="get_vertices" default="PackedVector3Array()">
+			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.
 		</member>
 	</members>
 </class>

doc/classes/NavigationRegion2D.xml

@@ -16,6 +16,13 @@
 		<link title="Using NavigationRegions">$DOCS_URL/tutorials/navigation/navigation_using_navigationregions.html</link>
 	</tutorials>
 	<methods>
+		<method name="get_avoidance_layer_value" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="layer_number" type="int" />
+			<description>
+				Returns whether or not the specified layer of the [member avoidance_layers] bitmask is enabled, given a [param layer_number] between 1 and 32.
+			</description>
+		</method>
 		<method name="get_navigation_layer_value" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="layer_number" type="int" />
@@ -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.
 			</description>
 		</method>
+		<method name="set_avoidance_layer_value">
+			<return type="void" />
+			<param index="0" name="layer_number" type="int" />
+			<param index="1" name="value" type="bool" />
+			<description>
+				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.
+			</description>
+		</method>
 		<method name="set_navigation_layer_value">
 			<return type="void" />
 			<param index="0" name="layer_number" type="int" />
@@ -39,6 +54,13 @@
 		</method>
 	</methods>
 	<members>
+		<member name="avoidance_layers" type="int" setter="set_avoidance_layers" getter="get_avoidance_layers" default="1">
+			A bitfield determining all avoidance layers for the avoidance constrain.
+		</member>
+		<member name="constrain_avoidance" type="bool" setter="set_constrain_avoidance" getter="get_constrain_avoidance" default="false">
+			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.
+		</member>
 		<member name="enabled" type="bool" setter="set_enabled" getter="is_enabled" default="true">
 			Determines if the [NavigationRegion2D] is enabled or disabled.
 		</member>

doc/classes/NavigationServer2D.xml

@@ -24,6 +24,13 @@
 				Creates the agent.
 			</description>
 		</method>
+		<method name="agent_get_avoidance_enabled" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="agent" type="RID" />
+			<description>
+				Return [code]true[/code] if the specified [param agent] uses avoidance.
+			</description>
+		</method>
 		<method name="agent_get_map" qualifiers="const">
 			<return type="RID" />
 			<param index="0" name="agent" type="RID" />
@@ -38,13 +45,46 @@
 				Returns true if the map got changed the previous frame.
 			</description>
 		</method>
-		<method name="agent_set_callback">
+		<method name="agent_set_avoidance_callback">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
 			<param index="1" name="callback" type="Callable" />
 			<description>
-				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].
+			</description>
+		</method>
+		<method name="agent_set_avoidance_enabled">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="enabled" type="bool" />
+			<description>
+				If [param enabled] is [code]true[/code] the specified [param agent] uses avoidance.
+			</description>
+		</method>
+		<method name="agent_set_avoidance_layers">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="layers" type="int" />
+			<description>
+				Set the agent's [code]avoidance_layers[/code] bitmask.
+			</description>
+		</method>
+		<method name="agent_set_avoidance_mask">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="mask" type="int" />
+			<description>
+				Set the agent's [code]avoidance_mask[/code] bitmask.
+			</description>
+		</method>
+		<method name="agent_set_avoidance_priority">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="priority" type="float" />
+			<description>
+				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.
 			</description>
 		</method>
 		<method name="agent_set_map">
@@ -95,20 +135,20 @@
 				Sets the radius of the agent.
 			</description>
 		</method>
-		<method name="agent_set_target_velocity">
+		<method name="agent_set_time_horizon_agents">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
-			<param index="1" name="target_velocity" type="Vector2" />
+			<param index="1" name="time_horizon" type="float" />
 			<description>
-				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.
 			</description>
 		</method>
-		<method name="agent_set_time_horizon">
+		<method name="agent_set_time_horizon_obstacles">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
-			<param index="1" name="time" type="float" />
+			<param index="1" name="time_horizon" type="float" />
 			<description>
-				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.
 			</description>
 		</method>
 		<method name="agent_set_velocity">
@@ -116,7 +156,15 @@
 			<param index="0" name="agent" type="RID" />
 			<param index="1" name="velocity" type="Vector2" />
 			<description>
-				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.
+			</description>
+		</method>
+		<method name="agent_set_velocity_forced">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="velocity" type="Vector2" />
+			<description>
+				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.
 			</description>
 		</method>
 		<method name="free_rid">
@@ -331,6 +379,13 @@
 				Returns all navigation link [RID]s that are currently assigned to the requested navigation [param map].
 			</description>
 		</method>
+		<method name="map_get_obstacles" qualifiers="const">
+			<return type="RID[]" />
+			<param index="0" name="map" type="RID" />
+			<description>
+				Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [code]map[/code].
+			</description>
+		</method>
 		<method name="map_get_path" qualifiers="const">
 			<return type="PackedVector2Array" />
 			<param index="0" name="map" type="RID" />
@@ -388,6 +443,50 @@
 				Set the map's link connection radius used to connect links to navigation polygons.
 			</description>
 		</method>
+		<method name="obstacle_create">
+			<return type="RID" />
+			<description>
+				Creates a new navigation obstacle.
+			</description>
+		</method>
+		<method name="obstacle_get_map" qualifiers="const">
+			<return type="RID" />
+			<param index="0" name="obstacle" type="RID" />
+			<description>
+				Returns the navigation map [RID] the requested [param obstacle] is currently assigned to.
+			</description>
+		</method>
+		<method name="obstacle_set_avoidance_layers">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="layers" type="int" />
+			<description>
+			</description>
+		</method>
+		<method name="obstacle_set_map">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="map" type="RID" />
+			<description>
+				Sets the navigation map [RID] for the obstacle.
+			</description>
+		</method>
+		<method name="obstacle_set_position">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="position" type="Vector2" />
+			<description>
+				Sets the position of the obstacle in world space.
+			</description>
+		</method>
+		<method name="obstacle_set_vertices">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="vertices" type="PackedVector2Array" />
+			<description>
+				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.
+			</description>
+		</method>
 		<method name="query_path" qualifiers="const">
 			<return type="void" />
 			<param index="0" name="parameters" type="NavigationPathQueryParameters2D" />

doc/classes/NavigationServer3D.xml

@@ -24,6 +24,13 @@
 				Creates the agent.
 			</description>
 		</method>
+		<method name="agent_get_avoidance_enabled" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="agent" type="RID" />
+			<description>
+				Returns [code]true[/code] if the provided [param agent] has avoidance enabled.
+			</description>
+		</method>
 		<method name="agent_get_map" qualifiers="const">
 			<return type="RID" />
 			<param index="0" name="agent" type="RID" />
@@ -31,6 +38,13 @@
 				Returns the navigation map [RID] the requested [param agent] is currently assigned to.
 			</description>
 		</method>
+		<method name="agent_get_use_3d_avoidance" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="agent" type="RID" />
+			<description>
+				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).
+			</description>
+		</method>
 		<method name="agent_is_map_changed" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="agent" type="RID" />
@@ -38,13 +52,54 @@
 				Returns true if the map got changed the previous frame.
 			</description>
 		</method>
-		<method name="agent_set_callback">
+		<method name="agent_set_avoidance_callback">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
 			<param index="1" name="callback" type="Callable" />
 			<description>
-				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].
+			</description>
+		</method>
+		<method name="agent_set_avoidance_enabled">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="enabled" type="bool" />
+			<description>
+				If [param enabled] the provided [param agent] calculates avoidance.
+			</description>
+		</method>
+		<method name="agent_set_avoidance_layers">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="layers" type="int" />
+			<description>
+				Set the agent's [code]avoidance_layers[/code] bitmask.
+			</description>
+		</method>
+		<method name="agent_set_avoidance_mask">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="mask" type="int" />
+			<description>
+				Set the agent's [code]avoidance_mask[/code] bitmask.
+			</description>
+		</method>
+		<method name="agent_set_avoidance_priority">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="priority" type="float" />
+			<description>
+				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.
+			</description>
+		</method>
+		<method name="agent_set_height">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="height" type="float" />
+			<description>
+				Updates the provided [param agent] [param height].
 			</description>
 		</method>
 		<method name="agent_set_map">
@@ -95,20 +150,30 @@
 				Sets the radius of the agent.
 			</description>
 		</method>
-		<method name="agent_set_target_velocity">
+		<method name="agent_set_time_horizon_agents">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="time_horizon" type="float" />
+			<description>
+				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.
+			</description>
+		</method>
+		<method name="agent_set_time_horizon_obstacles">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
-			<param index="1" name="target_velocity" type="Vector3" />
+			<param index="1" name="time_horizon" type="float" />
 			<description>
-				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 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.
 			</description>
 		</method>
-		<method name="agent_set_time_horizon">
+		<method name="agent_set_use_3d_avoidance">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
-			<param index="1" name="time" type="float" />
+			<param index="1" name="enabled" type="bool" />
 			<description>
-				The minimal amount of time for which the agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive.
+				Sets 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.
 			</description>
 		</method>
 		<method name="agent_set_velocity">
@@ -116,7 +181,15 @@
 			<param index="0" name="agent" type="RID" />
 			<param index="1" name="velocity" type="Vector3" />
 			<description>
-				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.
+			</description>
+		</method>
+		<method name="agent_set_velocity_forced">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="velocity" type="Vector3" />
+			<description>
+				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.
 			</description>
 		</method>
 		<method name="free_rid">
@@ -356,6 +429,13 @@
 				Returns all navigation link [RID]s that are currently assigned to the requested navigation [param map].
 			</description>
 		</method>
+		<method name="map_get_obstacles" qualifiers="const">
+			<return type="RID[]" />
+			<param index="0" name="map" type="RID" />
+			<description>
+				Returns all navigation obstacle [RID]s that are currently assigned to the requested navigation [code]map[/code].
+			</description>
+		</method>
 		<method name="map_get_path" qualifiers="const">
 			<return type="PackedVector3Array" />
 			<param index="0" name="map" type="RID" />
@@ -428,6 +508,59 @@
 				Sets the map up direction.
 			</description>
 		</method>
+		<method name="obstacle_create">
+			<return type="RID" />
+			<description>
+				Creates a new obstacle.
+			</description>
+		</method>
+		<method name="obstacle_get_map" qualifiers="const">
+			<return type="RID" />
+			<param index="0" name="obstacle" type="RID" />
+			<description>
+				Returns the navigation map [RID] the requested [param obstacle] is currently assigned to.
+			</description>
+		</method>
+		<method name="obstacle_set_avoidance_layers">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="layers" type="int" />
+			<description>
+				Set the obstacles's [code]avoidance_layers[/code] bitmask.
+			</description>
+		</method>
+		<method name="obstacle_set_height">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="height" type="float" />
+			<description>
+				Sets the [param height] for the [param obstacle]. In 3D agents will ignore obstacles that are above or below them while using 2D avoidance.
+			</description>
+		</method>
+		<method name="obstacle_set_map">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="map" type="RID" />
+			<description>
+				Assigns the [param obstacle] to a navigation map.
+			</description>
+		</method>
+		<method name="obstacle_set_position">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="position" type="Vector3" />
+			<description>
+				Updates the [param position] in world space for the [param obstacle].
+			</description>
+		</method>
+		<method name="obstacle_set_vertices">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="vertices" type="PackedVector3Array" />
+			<description>
+				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.
+			</description>
+		</method>
 		<method name="query_path" qualifiers="const">
 			<return type="void" />
 			<param index="0" name="parameters" type="NavigationPathQueryParameters3D" />
@@ -590,6 +723,11 @@
 		</method>
 	</methods>
 	<signals>
+		<signal name="avoidance_debug_changed">
+			<description>
+				Emitted when avoidance debug settings are changed. Only available in debug builds.
+			</description>
+		</signal>
 		<signal name="map_changed">
 			<param index="0" name="map" type="RID" />
 			<description>

doc/classes/ProjectSettings.xml

@@ -579,6 +579,33 @@
 		<member name="debug/shader_language/warnings/unused_varying" type="bool" setter="" getter="" default="true">
 			When set to [code]true[/code], produces a warning when a varying is never used.
 		</member>
+		<member name="debug/shapes/avoidance/agents_radius_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.25)">
+			Color of the avoidance agents radius, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/enable_agents_radius" type="bool" setter="" getter="" default="true">
+			If enabled, displays avoidance agents radius when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/enable_obstacles_radius" type="bool" setter="" getter="" default="true">
+			If enabled, displays avoidance obstacles radius when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/enable_obstacles_static" type="bool" setter="" getter="" default="true">
+			If enabled, displays static avoidance obstacles when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/obstacles_radius_color" type="Color" setter="" getter="" default="Color(1, 0.5, 0, 0.25)">
+			Color of the avoidance obstacles radius, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/obstacles_static_edge_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
+			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.
+		</member>
+		<member name="debug/shapes/avoidance/obstacles_static_edge_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 1)">
+			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.
+		</member>
+		<member name="debug/shapes/avoidance/obstacles_static_face_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 0)">
+			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.
+		</member>
+		<member name="debug/shapes/avoidance/obstacles_static_face_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.5)">
+			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.
+		</member>
 		<member name="debug/shapes/collision/contact_color" type="Color" setter="" getter="" default="Color(1, 0.2, 0.1, 0.8)">
 			Color of the contact points between collision shapes, visible when "Visible Collision Shapes" is enabled in the Debug menu.
 		</member>
@@ -1777,6 +1804,102 @@
 		<member name="layer_names/3d_render/layer_20" type="String" setter="" getter="" default="&quot;&quot;">
 			Optional name for the 3D render layer 20. If left empty, the layer will display as "Layer 20".
 		</member>
+		<member name="layer_names/avoidance/layer_1" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 1. If left empty, the layer will display as "Layer 1".
+		</member>
+		<member name="layer_names/avoidance/layer_2" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 2. If left empty, the layer will display as "Layer 2".
+		</member>
+		<member name="layer_names/avoidance/layer_3" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 3. If left empty, the layer will display as "Layer 3".
+		</member>
+		<member name="layer_names/avoidance/layer_4" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 4. If left empty, the layer will display as "Layer 4".
+		</member>
+		<member name="layer_names/avoidance/layer_5" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 5. If left empty, the layer will display as "Layer 5".
+		</member>
+		<member name="layer_names/avoidance/layer_6" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 6. If left empty, the layer will display as "Layer 6".
+		</member>
+		<member name="layer_names/avoidance/layer_7" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 7. If left empty, the layer will display as "Layer 7".
+		</member>
+		<member name="layer_names/avoidance/layer_8" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 8. If left empty, the layer will display as "Layer 8".
+		</member>
+		<member name="layer_names/avoidance/layer_9" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 9. If left empty, the layer will display as "Layer 9".
+		</member>
+		<member name="layer_names/avoidance/layer_10" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 10. If left empty, the layer will display as "Layer 10".
+		</member>
+		<member name="layer_names/avoidance/layer_11" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 11. If left empty, the layer will display as "Layer 11".
+		</member>
+		<member name="layer_names/avoidance/layer_12" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 12. If left empty, the layer will display as "Layer 12".
+		</member>
+		<member name="layer_names/avoidance/layer_13" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 13. If left empty, the layer will display as "Layer 13".
+		</member>
+		<member name="layer_names/avoidance/layer_14" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 14. If left empty, the layer will display as "Layer 14".
+		</member>
+		<member name="layer_names/avoidance/layer_15" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 15. If left empty, the layer will display as "Layer 15".
+		</member>
+		<member name="layer_names/avoidance/layer_16" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 16. If left empty, the layer will display as "Layer 16".
+		</member>
+		<member name="layer_names/avoidance/layer_17" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 17. If left empty, the layer will display as "Layer 17".
+		</member>
+		<member name="layer_names/avoidance/layer_18" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 18. If left empty, the layer will display as "Layer 18".
+		</member>
+		<member name="layer_names/avoidance/layer_19" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 19. If left empty, the layer will display as "Layer 19".
+		</member>
+		<member name="layer_names/avoidance/layer_20" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 20. If left empty, the layer will display as "Layer 20".
+		</member>
+		<member name="layer_names/avoidance/layer_21" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 21. If left empty, the layer will display as "Layer 21".
+		</member>
+		<member name="layer_names/avoidance/layer_22" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 22. If left empty, the layer will display as "Layer 22".
+		</member>
+		<member name="layer_names/avoidance/layer_23" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 23. If left empty, the layer will display as "Layer 23".
+		</member>
+		<member name="layer_names/avoidance/layer_24" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 24. If left empty, the layer will display as "Layer 24".
+		</member>
+		<member name="layer_names/avoidance/layer_25" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 25. If left empty, the layer will display as "Layer 25".
+		</member>
+		<member name="layer_names/avoidance/layer_26" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 26. If left empty, the layer will display as "Layer 26".
+		</member>
+		<member name="layer_names/avoidance/layer_27" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 27. If left empty, the layer will display as "Layer 27".
+		</member>
+		<member name="layer_names/avoidance/layer_28" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 28. If left empty, the layer will display as "Layer 28".
+		</member>
+		<member name="layer_names/avoidance/layer_29" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 29. If left empty, the layer will display as "Layer 29".
+		</member>
+		<member name="layer_names/avoidance/layer_30" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 30. If left empty, the layer will display as "Layer 30".
+		</member>
+		<member name="layer_names/avoidance/layer_31" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 31. If left empty, the layer will display as "Layer 31".
+		</member>
+		<member name="layer_names/avoidance/layer_32" type="String" setter="" getter="" default="&quot;&quot;">
+			Optional name for the navigation avoidance layer 32. If left empty, the layer will display as "Layer 32".
+		</member>
 		<member name="memory/limits/message_queue/max_size_mb" type="int" setter="" getter="" default="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.
 		</member>
@@ -1801,6 +1924,12 @@
 		<member name="navigation/3d/default_link_connection_radius" type="float" setter="" getter="" default="1.0">
 			Default link connection radius for 3D navigation maps. See [method NavigationServer3D.map_set_link_connection_radius].
 		</member>
+		<member name="navigation/avoidance/thread_model/avoidance_use_high_priority_threads" type="bool" setter="" getter="" default="true">
+			If enabled and avoidance calculations use multiple threads the threads run with high priority.
+		</member>
+		<member name="navigation/avoidance/thread_model/avoidance_use_multiple_threads" type="bool" setter="" getter="" default="true">
+			If enabled the avoidance calculations use multiple threads.
+		</member>
 		<member name="network/limits/debugger/max_chars_per_second" type="int" setter="" getter="" default="32768">
 			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.
 		</member>

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<String> 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
 				}

editor/editor_properties.h

@@ -308,6 +308,7 @@ public:
 		LAYER_PHYSICS_3D,
 		LAYER_RENDER_3D,
 		LAYER_NAVIGATION_3D,
+		LAYER_AVOIDANCE,
 	};
 
 private:

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);

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);
 	}

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,

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<NavigationObstacle2D>(p_polygon);
+}
+
+void NavigationObstacle2DEditor::_action_add_polygon(const Variant &p_polygon) {
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(node, "set_vertices", p_polygon);
+	undo_redo->add_undo_method(node, "set_vertices", node->get_vertices());
+}
+
+void NavigationObstacle2DEditor::_action_remove_polygon(int p_idx) {
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(node, "set_vertices", Variant(Vector<Vector2>()));
+	undo_redo->add_undo_method(node, "set_vertices", node->get_vertices());
+}
+
+void NavigationObstacle2DEditor::_action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) {
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(node, "set_vertices", p_polygon);
+	undo_redo->add_undo_method(node, "set_vertices", node->get_vertices());
+}
+
+NavigationObstacle2DEditor::NavigationObstacle2DEditor() {}
+
+NavigationObstacle2DEditorPlugin::NavigationObstacle2DEditorPlugin() :
+		AbstractPolygon2DEditorPlugin(memnew(NavigationObstacle2DEditor), "NavigationObstacle2D") {
+}

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

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<int> 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<InputEvent> &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<InputEventMouseButton> 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<InputEventMouseMotion> 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<Vector3> 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<NavigationObstacle3D>(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<StandardMaterial3D>(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<StandardMaterial3D>(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<Texture2D> 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<Node>(p_object));
+}
+
+bool NavigationObstacle3DEditorPlugin::handles(Object *p_object) const {
+	return Object::cast_to<NavigationObstacle3D>(p_object);
+}
+
+void NavigationObstacle3DEditorPlugin::make_visible(bool p_visible) {
+	if (p_visible) {
+		obstacle_editor->show();
+	} else {
+		obstacle_editor->hide();
+		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() {
+}

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<StandardMaterial3D> line_material;
+	Ref<StandardMaterial3D> handle_material;
+
+	Panel *panel = nullptr;
+	NavigationObstacle3D *obstacle_node = nullptr;
+	Ref<ImmediateMesh> point_lines_mesh;
+	MeshInstance3D *point_lines_meshinstance = nullptr;
+	MeshInstance3D *point_handles_meshinstance = nullptr;
+	Ref<ArrayMesh> 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<InputEvent> &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<InputEvent> &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

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<MeshInstance3DEditorPlugin>();
 	EditorPlugins::add_by_type<MeshLibraryEditorPlugin>();
 	EditorPlugins::add_by_type<MultiMeshEditorPlugin>();
+	EditorPlugins::add_by_type<NavigationObstacle3DEditorPlugin>();
 	EditorPlugins::add_by_type<OccluderInstance3DEditorPlugin>();
 	EditorPlugins::add_by_type<PackedSceneEditorPlugin>();
 	EditorPlugins::add_by_type<Path3DEditorPlugin>();
@@ -213,6 +216,7 @@ void register_editor_types() {
 	EditorPlugins::add_by_type<LightOccluder2DEditorPlugin>();
 	EditorPlugins::add_by_type<Line2DEditorPlugin>();
 	EditorPlugins::add_by_type<NavigationLink2DEditorPlugin>();
+	EditorPlugins::add_by_type<NavigationObstacle2DEditorPlugin>();
 	EditorPlugins::add_by_type<NavigationPolygonEditorPlugin>();
 	EditorPlugins::add_by_type<Path2DEditorPlugin>();
 	EditorPlugins::add_by_type<Polygon2DEditorPlugin>();

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 <ms>                Simulate high CPU load (delay each frame by <ms> 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
 

modules/gdscript/doc_classes/@GDScript.xml

@@ -458,6 +458,16 @@
 				[/codeblock]
 			</description>
 		</annotation>
+		<annotation name="@export_flags_avoidance">
+			<return type="void" />
+			<description>
+				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]
+			</description>
+		</annotation>
 		<annotation name="@export_global_dir">
 			<return type="void" />
 			<description>

modules/gdscript/gdscript_parser.cpp

@@ -104,6 +104,7 @@ GDScriptParser::GDScriptParser() {
 	register_annotation(MethodInfo("@export_flags_3d_render"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations<PROPERTY_HINT_LAYERS_3D_RENDER, Variant::INT>);
 	register_annotation(MethodInfo("@export_flags_3d_physics"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations<PROPERTY_HINT_LAYERS_3D_PHYSICS, Variant::INT>);
 	register_annotation(MethodInfo("@export_flags_3d_navigation"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations<PROPERTY_HINT_LAYERS_3D_NAVIGATION, Variant::INT>);
+	register_annotation(MethodInfo("@export_flags_avoidance"), AnnotationInfo::VARIABLE, &GDScriptParser::export_annotations<PROPERTY_HINT_LAYERS_AVOIDANCE, Variant::INT>);
 	// Export grouping annotations.
 	register_annotation(MethodInfo("@export_category", PropertyInfo(Variant::STRING, "name")), AnnotationInfo::STANDALONE, &GDScriptParser::export_group_annotations<PROPERTY_USAGE_CATEGORY>);
 	register_annotation(MethodInfo("@export_group", PropertyInfo(Variant::STRING, "name"), PropertyInfo(Variant::STRING, "prefix")), AnnotationInfo::STANDALONE, &GDScriptParser::export_group_annotations<PROPERTY_USAGE_GROUP>, varray(""));

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);
 		}

modules/navigation/SCsub

@@ -33,12 +33,14 @@ 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]
 
@@ -48,9 +50,24 @@ if env["builtin_rvo2"]:
     env_thirdparty.disable_warnings()
     env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
 
+# RVO 3D Thirdparty source files
+if env["builtin_rvo2_3d"]:
+    thirdparty_dir = "#thirdparty/rvo2/rvo2_3d/"
+    thirdparty_sources = [
+        "Agent3d.cpp",
+        "KdTree3d.cpp",
+        "RVOSimulator3d.cpp",
+    ]
+    thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
 
-env.modules_sources += thirdparty_obj
+    env_navigation.Prepend(CPPPATH=[thirdparty_dir])
 
+    env_thirdparty = env_navigation.Clone()
+    env_thirdparty.disable_warnings()
+    env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+
+
+env.modules_sources += thirdparty_obj
 
 # Godot source files
 

modules/navigation/godot_navigation_server.cpp

@@ -271,6 +271,18 @@ TypedArray<RID> GodotNavigationServer::map_get_agents(RID p_map) const {
 	return agents_rids;
 }
 
+TypedArray<RID> GodotNavigationServer::map_get_obstacles(RID p_map) const {
+	TypedArray<RID> obstacles_rids;
+	const NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND_V(map == nullptr, obstacles_rids);
+	const LocalVector<NavObstacle *> 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_max_speed, RID, p_agent, real_t, p_max_speed) {
+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->get_agent()->maxSpeed_ = p_max_speed;
+	agent->set_height(p_height);
 }
 
-COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) {
+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()->velocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+	agent->set_max_speed(p_max_speed);
 }
 
-COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity) {
+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()->prefVelocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+	agent->set_velocity(p_velocity);
 }
 
-COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) {
+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()->position_ = RVO::Vector3(p_position.x, p_position.y, p_position.z);
+	agent->set_velocity_forced(p_velocity);
 }
 
-COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore) {
+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()->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<GodotNavigationServer *>(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<Vector3> &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).");
 	}

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<NavMap> map_owner;
 	mutable RID_Owner<NavRegion> region_owner;
 	mutable RID_Owner<NavAgent> agent_owner;
+	mutable RID_Owner<NavObstacle> obstacle_owner;
 
 	bool active = true;
 	LocalVector<NavMap *> active_maps;
@@ -121,6 +123,7 @@ public:
 	virtual TypedArray<RID> map_get_links(RID p_map) const override;
 	virtual TypedArray<RID> map_get_regions(RID p_map) const override;
 	virtual TypedArray<RID> map_get_agents(RID p_map) const override;
+	virtual TypedArray<RID> map_get_obstacles(RID p_map) const override;
 
 	virtual void map_force_update(RID p_map) override;
 
@@ -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<Vector3> &p_vertices) override;
+	COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers);
 
 	COMMAND_1(free, RID, p_object);
 

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;
 }

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 <Agent.h>
+#include <Agent2d.h>
+#include <Agent3d.h>
 
 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();
+
+	void set_avoidance_enabled(bool p_enabled);
+	bool is_avoidance_enabled() { return avoidance_enabled; }
 
-	RVO::Agent *get_agent() {
-		return &agent;
-	}
+	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 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;
 
-	void dispatch_callback();
+private:
+	void _update_rvo_agent_properties();
 };
 
 #endif // NAV_AGENT_H

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 <algorithm>
+
+#include <Obstacle2d.h>
 
 #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<RVO::Agent *> 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;
+		}
+	}
+	// Do we have modified agent arrays?
+	for (NavAgent *agent : agents) {
+		if (agent->check_dirty()) {
+			agents_dirty = true;
 		}
-		rvo.buildAgentTree(raw_agents);
+	}
+
+	// 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<RVO2D::Obstacle2D *> 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<Vector3> &_obstacle_vertices = obstacle->get_vertices();
+
+		if (_obstacle_vertices.size() < 2) {
+			continue;
+		}
+
+		std::vector<RVO2D::Vector2> rvo_2d_vertices;
+		rvo_2d_vertices.reserve(_obstacle_vertices.size());
+
+		uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
+
+		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<RVO2D::Agent2D *> 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<RVO3D::Agent3D *> 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<gd::NavigationPoly> &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() {

modules/navigation/nav_map.h

@@ -38,11 +38,16 @@
 #include "core/templates/rb_map.h"
 #include "nav_utils.h"
 
-#include <KdTree.h>
+#include <KdTree2d.h>
+#include <RVOSimulator2d.h>
+
+#include <KdTree3d.h>
+#include <RVOSimulator3d.h>
 
 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<gd::Polygon> polygons;
 
-	/// Rvo world
-	RVO::KdTree rvo;
+	/// RVO avoidance worlds
+	RVO2D::RVOSimulator2D rvo_simulation_2d;
+	RVO3D::RVOSimulator3D rvo_simulation_3d;
+
+	/// avoidance controlled agents
+	LocalVector<NavAgent *> active_2d_avoidance_agents;
+	LocalVector<NavAgent *> active_3d_avoidance_agents;
 
-	/// Is agent array modified?
-	bool agents_dirty = false;
+	/// dirty flag when one of the agent's arrays are modified
+	bool agents_dirty = true;
 
 	/// All the Agents (even the controlled one)
 	LocalVector<NavAgent *> agents;
 
-	/// Controlled agents
-	LocalVector<NavAgent *> controlled_agents;
+	/// All the avoidance obstacles (both static and dynamic)
+	LocalVector<NavObstacle *> 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<NavObstacle *> &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<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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

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<Vector3> &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;
+}

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<Vector3> 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<Vector3> &p_vertices);
+	const Vector<Vector3> &get_vertices() const { return vertices; }
+
+	bool is_map_changed();
+
+	void set_avoidance_layers(uint32_t p_layers);
+	uint32_t get_avoidance_layers() const { return avoidance_layers; };
+
+	bool check_dirty();
+};
+
+#endif // NAV_OBSTACLE_H

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<NavigationPathQueryResult2D>();
 	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<Node2D>(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) {

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<NavigationPathQueryParameters2D> navigation_query;
 	Ref<NavigationPathQueryResult2D> 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<NavigationPathQueryResult2D> get_current_navigation_result() const {
-		return navigation_result;
-	}
-	const Vector<Vector2> &get_current_navigation_path() const {
-		return navigation_result->get_path();
-	}
-	int get_current_navigation_path_index() const {
-		return navigation_path_index;
-	}
+	Ref<NavigationPathQueryResult2D> get_current_navigation_result() const { return navigation_result; }
+
+	const Vector<Vector2> &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

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
-}
 
-PackedStringArray NavigationObstacle2D::get_configuration_warnings() const {
-	PackedStringArray warnings = Node::get_configuration_warnings();
+	NavigationServer2D::get_singleton()->free(obstacle);
+	obstacle = RID();
 
-	if (!Object::cast_to<Node2D>(get_parent())) {
-		warnings.push_back(RTR("The NavigationObstacle2D only serves to provide collision avoidance to a Node2D object."));
-	}
-
-	if (Object::cast_to<StaticBody2D>(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;
+	NavigationServer2D::get_singleton()->free(fake_agent);
+	fake_agent = RID();
 }
 
-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::set_vertices(const Vector<Vector2> &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();
+	}
 }
 
-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());
+void NavigationObstacle2D::set_navigation_map(RID p_navigation_map) {
+	if (map_override == p_navigation_map) {
+		return;
 	}
+	map_override = p_navigation_map;
+	_update_map(map_override);
 }
 
-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<CollisionShape2D>(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;
-		}
+RID NavigationObstacle2D::get_navigation_map() const {
+	if (map_override.is_valid()) {
+		return map_override;
+	} else if (is_inside_tree()) {
+		return get_world_2d()->get_navigation_map();
 	}
-	return 1.0; // Never a 0 radius
+	return RID();
 }
 
-void NavigationObstacle2D::set_agent_parent(Node *p_agent_parent) {
-	if (parent_node2d == p_agent_parent) {
+void NavigationObstacle2D::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;
 	}
 
-	if (Object::cast_to<Node2D>(p_agent_parent) != nullptr) {
-		parent_node2d = Object::cast_to<Node2D>(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());
+	radius = p_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::_avoidance_done(Vector3 p_new_velocity) {
-	// Dummy function as obstacle requires a valid Callback to be added to avoidance simulation.
+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);
 }
 
-void NavigationObstacle2D::set_navigation_map(RID p_navigation_map) {
-	if (map_override == p_navigation_map) {
-		return;
+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);
+}
 
-	map_override = p_navigation_map;
+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));
+}
 
-	NavigationServer2D::get_singleton()->agent_set_map(agent, map_override);
+void NavigationObstacle2D::set_velocity(const Vector2 p_velocity) {
+	velocity = p_velocity;
+	velocity_submitted = true;
 }
 
-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();
+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);
 	}
-	return RID();
 }
 
-void NavigationObstacle2D::set_estimate_radius(bool p_estimate_radius) {
-	if (estimate_radius == p_estimate_radius) {
-		return;
+#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
 
-	estimate_radius = p_estimate_radius;
+#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());
 
-	notify_property_list_changed();
-	reevaluate_agent_radius();
-}
+		Color debug_static_obstacle_face_color;
 
-void NavigationObstacle2D::set_radius(real_t p_radius) {
-	ERR_FAIL_COND_MSG(p_radius <= 0.0, "Radius must be greater than 0.");
-	if (Math::is_equal_approx(radius, p_radius)) {
-		return;
-	}
+		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();
+		}
 
-	radius = p_radius;
+		Vector<Vector2> debug_obstacle_polygon_vertices = get_vertices();
+
+		Vector<Color> 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<Vector2> 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());
 
-	reevaluate_agent_radius();
+		Vector<Color> 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

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;
+
+	real_t radius = 0.0;
+
+	Vector<Vector2> vertices;
+
+	RID fake_agent;
+	uint32_t avoidance_layers = 1;
+
+	Transform2D previous_transform;
+
+	Vector2 velocity;
+	Vector2 previous_velocity;
+	bool velocity_submitted = false;
 
-	bool estimate_radius = true;
-	real_t radius = 1.0;
+#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; }
+
+	void set_vertices(const Vector<Vector2> &p_vertices);
+	const Vector<Vector2> &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;
 
-	PackedStringArray get_configuration_warnings() const override;
+	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

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<NavigationPolygon> _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<Vector2> &_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<Vector2> 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));
+}

scene/2d/navigation_region_2d.h

@@ -43,11 +43,16 @@ class NavigationRegion2D : public Node2D {
 	real_t travel_cost = 1.0;
 	Ref<NavigationPolygon> navigation_polygon;
 
+	bool constrain_avoidance = false;
+	LocalVector<RID> 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<NavigationPolygon> &p_navigation_polygon);
 	Ref<NavigationPolygon> 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

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<NavigationPathQueryParameters3D>();
@@ -243,6 +328,12 @@ NavigationAgent3D::NavigationAgent3D() {
 	navigation_result = Ref<NavigationPathQueryResult3D>();
 	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<Node3D>(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;
-	}
-
-	ignore_y = p_ignore_y;
+void NavigationAgent3D::set_path_height_offset(real_t p_path_height_offset) {
+	path_height_offset = p_path_height_offset;
+}
 
-	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<RID> &navigation_path_rids = navigation_result->get_path_rids();
 		const Vector<int64_t> &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) {

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<NavigationPathQueryParameters3D> navigation_query;
 	Ref<NavigationPathQueryResult3D> 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<ArrayMesh> debug_path_mesh;
 	Ref<StandardMaterial3D> debug_agent_path_line_custom_material;
 	Ref<StandardMaterial3D> 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<NavigationPathQueryResult3D> get_current_navigation_result() const {
-		return navigation_result;
-	}
-	const Vector<Vector3> &get_current_navigation_path() const {
-		return navigation_result->get_path();
-	}
-	int get_current_navigation_path_index() const {
-		return navigation_path_index;
-	}
+	Ref<NavigationPathQueryResult3D> get_current_navigation_result() const { return navigation_result; }
+
+	const Vector<Vector3> &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

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);
 		}

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);
-
-				PhysicsBody3D *rigid = Object::cast_to<PhysicsBody3D>(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 (is_inside_tree()) {
+				_update_position(get_global_transform().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)) {
+						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();
+
+	NavigationServer3D::get_singleton()->free(fake_agent);
+	fake_agent = RID();
 
-	if (!Object::cast_to<Node3D>(get_parent())) {
-		warnings.push_back(RTR("The NavigationObstacle3D only serves to provide collision avoidance to a Node3D inheriting parent object."));
+#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<StaticBody3D>(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);
+	}
+	if (static_obstacle_debug_mesh.is_valid()) {
+		RenderingServer::get_singleton()->free(static_obstacle_debug_mesh->get_rid());
 	}
+#endif // DEBUG_ENABLED
+}
 
-	return warnings;
+void NavigationObstacle3D::set_vertices(const Vector<Vector3> &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::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::set_navigation_map(RID p_navigation_map) {
+	if (map_override == p_navigation_map) {
+		return;
+	}
+	map_override = p_navigation_map;
+	_update_map(map_override);
 }
 
-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());
+RID NavigationObstacle3D::get_navigation_map() const {
+	if (map_override.is_valid()) {
+		return map_override;
+	} else if (is_inside_tree()) {
+		return get_world_3d()->get_navigation_map();
 	}
+	return RID();
 }
 
-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<CollisionShape3D>(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.");
-			}
-		}
+void NavigationObstacle3D::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;
+	}
 
-		Vector3 s = parent_node3d->get_global_transform().basis.get_scale();
-		max_radius *= MAX(s.x, MAX(s.y, s.z));
+	radius = p_radius;
+	NavigationServer3D::get_singleton()->agent_set_avoidance_enabled(fake_agent, radius > 0.0);
+	NavigationServer3D::get_singleton()->agent_set_radius(fake_agent, radius);
 
-		if (max_radius > 0.0) {
-			return max_radius;
-		}
-	}
-	return 1.0; // Never a 0 radius
+#ifdef DEBUG_ENABLED
+	_update_fake_agent_radius_debug();
+#endif // DEBUG_ENABLED
 }
 
-void NavigationObstacle3D::set_agent_parent(Node *p_agent_parent) {
-	if (parent_node3d == p_agent_parent) {
+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;
 	}
 
-	if (Object::cast_to<Node3D>(p_agent_parent) != nullptr) {
-		parent_node3d = Object::cast_to<Node3D>(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();
+	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 {
-		parent_node3d = nullptr;
-		NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID());
-		NavigationServer3D::get_singleton()->agent_set_callback(agent, Callable());
+		avoidance_layers_new &= ~(1 << (p_layer_number - 1));
 	}
+	set_avoidance_layers(avoidance_layers_new);
 }
 
-void NavigationObstacle3D::_avoidance_done(Vector3 p_new_velocity) {
-	// Dummy function as obstacle requires a valid Callback to be added to avoidance simulation.
+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_navigation_map(RID p_navigation_map) {
-	if (map_override == p_navigation_map) {
-		return;
-	}
+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();
+}
 
-	map_override = p_navigation_map;
+void NavigationObstacle3D::set_velocity(const Vector3 p_velocity) {
+	velocity = p_velocity;
+	velocity_submitted = true;
+}
 
-	NavigationServer3D::get_singleton()->agent_set_map(agent, map_override);
+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;
 }
 
-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();
+void NavigationObstacle3D::_update_position(const Vector3 p_position) {
+	if (vertices.size() > 0) {
+		NavigationServer3D::get_singleton()->obstacle_set_position(obstacle, p_position);
 	}
-	return RID();
+	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);
 }
 
-void NavigationObstacle3D::set_estimate_radius(bool p_estimate_radius) {
-	if (estimate_radius == p_estimate_radius) {
+#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;
 	}
 
-	estimate_radius = p_estimate_radius;
+	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<ArrayMesh>(memnew(ArrayMesh));
+	}
+	fake_agent_radius_debug_mesh->clear_surfaces();
 
-	notify_property_list_changed();
-	reevaluate_agent_radius();
+	Vector<Vector3> face_vertex_array;
+	Vector<int> 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<StandardMaterial3D> 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;
+	}
 
-void NavigationObstacle3D::set_radius(real_t p_radius) {
-	ERR_FAIL_COND_MSG(p_radius <= 0.0, "Radius must be greater than 0.");
-	if (Math::is_equal_approx(radius, p_radius)) {
+	if (is_debug_enabled == false) {
+		if (static_obstacle_debug_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(static_obstacle_debug_instance, false);
+		}
 		return;
 	}
 
-	radius = p_radius;
+	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<ArrayMesh>(memnew(ArrayMesh));
+	}
+	static_obstacle_debug_mesh->clear_surfaces();
+
+	Vector<Vector2> polygon_2d_vertices;
+	polygon_2d_vertices.resize(vertices.size());
+	Vector2 *polygon_2d_vertices_ptr = polygon_2d_vertices.ptrw();
 
-	reevaluate_agent_radius();
+	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<int> 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<Vector3> face_vertex_array;
+	Vector<int> 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<Vector3> 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<StandardMaterial3D> face_material;
+	Ref<StandardMaterial3D> 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

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;
+
+	real_t height = 1.0;
+	real_t radius = 0.0;
+
+	Vector<Vector3> 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<ArrayMesh> fake_agent_radius_debug_mesh;
 
-	bool estimate_radius = true;
-	real_t radius = 1.0;
+	RID static_obstacle_debug_instance;
+	Ref<ArrayMesh> 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; }
+
+	void set_height(real_t p_height);
+	real_t get_height() const { return height; }
+
+	void set_vertices(const Vector<Vector3> &p_vertices);
+	const Vector<Vector3> &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; }
 
-	PackedStringArray get_configuration_warnings() const override;
+	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

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<NavigationMesh> &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);
 		}

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) {

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; }

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.
 	}

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<Vector3> vector_v2_to_v3(const Vector<Vector2> &d) {
+	Vector<Vector3> 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<Vector2> vector_v3_to_v2(const Vector<Vector3> &d) {
 	Vector<Vector2> 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<RID> FORWARD_1_C(map_get_regions, RID, p_map, rid_to_rid);
 
 TypedArray<RID> FORWARD_1_C(map_get_agents, RID, p_map, rid_to_rid);
 
+TypedArray<RID> 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<Vector2> &p_vertices) {
+	NavigationServer3D::get_singleton()->obstacle_set_vertices(p_obstacle, vector_v2_to_v3(p_vertices));
+}
 
 void NavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
 	ERR_FAIL_COND(!p_query_parameters.is_valid());

servers/navigation_server_2d.h

@@ -91,6 +91,7 @@ public:
 	virtual TypedArray<RID> map_get_links(RID p_map) const;
 	virtual TypedArray<RID> map_get_regions(RID p_map) const;
 	virtual TypedArray<RID> map_get_agents(RID p_map) const;
+	virtual TypedArray<RID> 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<Vector2> &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<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> 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

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<StandardMaterial3D> NavigationServer3D::get_debug_navigation_agent_path_line
 
 	Ref<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> NavigationServer3D::get_debug_navigation_agent_path_poin
 	}
 
 	Ref<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> NavigationServer3D::get_debug_navigation_agent_path_poin
 	return debug_navigation_agent_path_point_material;
 }
 
+Ref<StandardMaterial3D> 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<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> 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<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> 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<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> 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<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> 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<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<StandardMaterial3D> 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<StandardMaterial3D> material = Ref<StandardMaterial3D>(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<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result) const {

servers/navigation_server_3d.h

@@ -103,6 +103,7 @@ public:
 	virtual TypedArray<RID> map_get_links(RID p_map) const = 0;
 	virtual TypedArray<RID> map_get_regions(RID p_map) const = 0;
 	virtual TypedArray<RID> map_get_agents(RID p_map) const = 0;
+	virtual TypedArray<RID> 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<Vector3> &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<StandardMaterial3D> debug_navigation_geometry_edge_material;
 	Ref<StandardMaterial3D> debug_navigation_geometry_face_material;
 	Ref<StandardMaterial3D> debug_navigation_geometry_edge_disabled_material;
@@ -312,11 +350,24 @@ private:
 	Ref<StandardMaterial3D> debug_navigation_edge_connections_material;
 	Ref<StandardMaterial3D> debug_navigation_link_connections_material;
 	Ref<StandardMaterial3D> debug_navigation_link_connections_disabled_material;
+	Ref<StandardMaterial3D> debug_navigation_avoidance_agents_radius_material;
+	Ref<StandardMaterial3D> debug_navigation_avoidance_obstacles_radius_material;
+
+	Ref<StandardMaterial3D> debug_navigation_avoidance_static_obstacle_pushin_face_material;
+	Ref<StandardMaterial3D> debug_navigation_avoidance_static_obstacle_pushout_face_material;
+	Ref<StandardMaterial3D> debug_navigation_avoidance_static_obstacle_pushin_edge_material;
+	Ref<StandardMaterial3D> debug_navigation_avoidance_static_obstacle_pushout_edge_material;
 
 	Ref<StandardMaterial3D> debug_navigation_agent_path_line_material;
 	Ref<StandardMaterial3D> 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<StandardMaterial3D> get_debug_navigation_geometry_face_material();
 	Ref<StandardMaterial3D> get_debug_navigation_geometry_edge_material();
 	Ref<StandardMaterial3D> get_debug_navigation_geometry_face_disabled_material();
@@ -381,6 +459,14 @@ public:
 
 	Ref<StandardMaterial3D> get_debug_navigation_agent_path_line_material();
 	Ref<StandardMaterial3D> get_debug_navigation_agent_path_point_material();
+
+	Ref<StandardMaterial3D> get_debug_navigation_avoidance_agents_radius_material();
+	Ref<StandardMaterial3D> get_debug_navigation_avoidance_obstacles_radius_material();
+
+	Ref<StandardMaterial3D> get_debug_navigation_avoidance_static_obstacle_pushin_face_material();
+	Ref<StandardMaterial3D> get_debug_navigation_avoidance_static_obstacle_pushout_face_material();
+	Ref<StandardMaterial3D> get_debug_navigation_avoidance_static_obstacle_pushin_edge_material();
+	Ref<StandardMaterial3D> get_debug_navigation_avoidance_static_obstacle_pushout_edge_material();
 #endif // DEBUG_ENABLED
 };
 

servers/navigation_server_3d_dummy.h

@@ -57,6 +57,7 @@ public:
 	TypedArray<RID> map_get_links(RID p_map) const override { return TypedArray<RID>(); }
 	TypedArray<RID> map_get_regions(RID p_map) const override { return TypedArray<RID>(); }
 	TypedArray<RID> map_get_agents(RID p_map) const override { return TypedArray<RID>(); }
+	TypedArray<RID> map_get_obstacles(RID p_map) const override { return TypedArray<RID>(); }
 	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<Vector3> &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 {}

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

thirdparty/rvo2/patches/rvo2-godot-changes.patch

@@ -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<Plane> &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<Plane> &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 <utility>
- #include <vector>
- 
--#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<std::pair<float, const Agent *> > agentNeighbors_;
- 		std::vector<Plane> 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<Agent *> 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 <i>k</i>d-trees for agents in the simulation.
- 	 */
- 	class KdTree {
--	private:
-+	public:
- 		/**
- 		 * \brief   Defines an agent <i>k</i>d-tree node.
- 		 */
-@@ -90,12 +93,12 @@ namespace RVO {
- 		 * \brief   Constructs a <i>k</i>d-tree instance.
- 		 * \param   sim  The simulator instance.
- 		 */
--		explicit KdTree(RVOSimulator *sim);
-+		explicit KdTree();
- 
- 		/**
- 		 * \brief   Builds an agent <i>k</i>d-tree.
- 		 */
--		void buildAgentTree();
-+		void buildAgentTree(std::vector<Agent *> agents);
- 
- 		void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
- 
-@@ -110,7 +113,6 @@ namespace RVO {
- 
- 		std::vector<Agent *> agents_;
- 		std::vector<AgentTreeNode> 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 <cstddef>
- #include <ostream>
- 
--#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.

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#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<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
+			const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
+			const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::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<Line> &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<Line> &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<Line> &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<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(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);
+			}
+		}
+	}
+}

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#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<std::pair<float, const Agent2D *> > agentNeighbors_;
+		size_t maxNeighbors_;
+		float maxSpeed_;
+		float neighborDist_;
+		Vector2 newVelocity_;
+		std::vector<std::pair<float, const Obstacle2D *> > obstacleNeighbors_;
+		std::vector<Line> 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<Line> &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<Line> &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<Line> &lines, size_t numObstLines, size_t beginLine,
+						float radius, Vector2 &result);
+}
+
+#endif /* RVO2D_AGENT_H_ */

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#ifndef RVO2D_DEFINITIONS_H_
+#define RVO2D_DEFINITIONS_H_
+
+/**
+ * \file       Definitions.h
+ * \brief      Contains functions and constants used in multiple classes.
+ */
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#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_ */

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#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<Agent2D *> 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<Obstacle2D *> obstacles)
+	{
+		deleteObstacleTree(obstacleTree_);
+
+		obstacleTree_ = buildObstacleTreeRecursive(obstacles);
+	}
+
+
+	KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &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<Obstacle2D *> leftObstacles(minLeft);
+			std::vector<Obstacle2D *> 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));
+			}
+		}
+	}
+}

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#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 <i>k</i>d-trees for agents and static obstacles in the
+	 *             simulation.
+	 */
+	class KdTree2D {
+	public:
+		/**
+		 * \brief      Defines an agent <i>k</i>d-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 <i>k</i>d-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 <i>k</i>d-tree instance.
+		 * \param      sim             The simulator instance.
+		 */
+		explicit KdTree2D(RVOSimulator2D *sim);
+
+		/**
+		 * \brief      Destroys this kd-tree instance.
+		 */
+		~KdTree2D();
+
+		/**
+		 * \brief      Builds an agent <i>k</i>d-tree.
+		 */
+		void buildAgentTree(std::vector<Agent2D *> agents);
+
+		void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
+
+		/**
+		 * \brief      Builds an obstacle <i>k</i>d-tree.
+		 */
+		void buildObstacleTree(std::vector<Obstacle2D *> obstacles);
+
+		ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &
+													 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<Agent2D *> agents_;
+		std::vector<AgentTreeNode> agentTree_;
+		ObstacleTreeNode *obstacleTree_;
+		RVOSimulator2D *sim_;
+
+		static const size_t MAX_LEAF_SIZE = 10;
+
+		friend class Agent2D;
+		friend class RVOSimulator2D;
+	};
+}
+
+#endif /* RVO2D_KD_TREE_H_ */

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "Obstacle2d.h"
+#include "RVOSimulator2d.h"
+
+namespace RVO2D {
+	Obstacle2D::Obstacle2D() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { }
+}

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#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_ */

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "RVOSimulator2d.h"
+
+#include "Agent2d.h"
+#include "KdTree2d.h"
+#include "Obstacle2d.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#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<Vector2> &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<int>(agents_.size()); ++i) {
+			agents_[i]->computeNeighbors(this);
+			agents_[i]->computeNewVelocity(this);
+		}
+
+		for (int i = 0; i < static_cast<int>(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;
+	}
+}

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#ifndef RVO2D_RVO_SIMULATOR_H_
+#define RVO2D_RVO_SIMULATOR_H_
+
+/**
+ * \file       RVOSimulator2d.h
+ * \brief      Contains the RVOSimulator2D class.
+ */
+
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#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<size_t>::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<Vector2> &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<Agent2D *> agents_;
+		Agent2D *defaultAgent_;
+		float globalTime_;
+		KdTree2D *kdTree_;
+		std::vector<Obstacle2D *> obstacles_;
+		float timeStep_;
+
+		friend class Agent2D;
+		friend class KdTree2D;
+		friend class Obstacle2D;
+	};
+}
+
+#endif /* RVO2D_RVO_SIMULATOR_H_ */

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#ifndef RVO_VECTOR2_H_
+#define RVO_VECTOR2_H_
+
+/**
+ * \file       Vector2.h
+ * \brief      Contains the Vector2 class.
+ */
+
+#include <cmath>
+#include <ostream>
+
+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_ */

thirdparty/rvo2/Agent.cpp → thirdparty/rvo2/rvo2_3d/Agent3d.cpp

@@ -30,15 +30,15 @@
  * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-#include "Agent.h"
+#include "Agent3d.h"
 
 #include <cmath>
 #include <algorithm>
 
 #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<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
+	bool linearProgram1(const std::vector<Plane> &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<Plane> &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;
-
-			Vector3 relativePosition = other->position_ - position_;
-			Vector3 relativeVelocity = velocity_ - other->velocity_;
-			const float combinedRadius = radius_ + other->radius_;
+			const Agent3D *const other = agentNeighbors_[i].second;
 
-			// 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<Plane> &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<Plane> &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;

thirdparty/rvo2/Agent.h → 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
  *
- * <https://gamma.cs.unc.edu/RVO2/>
+ * <http://gamma.cs.unc.edu/RVO2/>
  */
 
 /**
@@ -41,77 +41,64 @@
 #include <utility>
 #include <vector>
 
+#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<std::pair<float, const Agent *> > agentNeighbors_;
+		float timeHorizonObst_;
+		std::vector<std::pair<float, const Agent3D *> > agentNeighbors_;
 		std::vector<Plane> 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;
 	};
 }
 

thirdparty/rvo2/Definitions.h → 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.

thirdparty/rvo2/KdTree.cpp → thirdparty/rvo2/rvo2_3d/KdTree3d.cpp

@@ -30,19 +30,20 @@
  * <https://gamma.cs.unc.edu/RVO2/>
  */
 
-#include "KdTree.h"
+#include "KdTree3d.h"
 
 #include <algorithm>
 
-#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<Agent *> agents)
+	void KdTree3D::buildAgentTree(std::vector<Agent3D *> 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) {

thirdparty/rvo2/KdTree.h → 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 <i>k</i>d-trees for agents in the simulation.
 	 */
-	class KdTree {
+	class KdTree3D {
 	public:
 		/**
 		 * \brief   Defines an agent <i>k</i>d-tree node.
 		 */
-		class AgentTreeNode {
+		class AgentTreeNode3D {
 		public:
 			/**
 			 * \brief   The beginning node number.
@@ -93,12 +90,12 @@ namespace RVO {
 		 * \brief   Constructs a <i>k</i>d-tree instance.
 		 * \param   sim  The simulator instance.
 		 */
-		explicit KdTree();
+		explicit KdTree3D(RVOSimulator3D *sim);
 
 		/**
 		 * \brief   Builds an agent <i>k</i>d-tree.
 		 */
-		void buildAgentTree(std::vector<Agent *> agents);
+		void buildAgentTree(std::vector<Agent3D *> 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<Agent *> agents_;
-		std::vector<AgentTreeNode> agentTree_;
+		std::vector<Agent3D *> agents_;
+		std::vector<AgentTreeNode3D> agentTree_;
+		RVOSimulator3D *sim_;
 
-		friend class Agent;
-		friend class RVOSimulator;
+		friend class Agent3D;
+		friend class RVOSimulator3D;
 	};
 }
 

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+#include "RVOSimulator3d.h"
+
+#ifdef _OPENMP
+#include <omp.h>
+#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<int>(agents_.size()); ++i) {
+			agents_[i]->computeNeighbors(this);
+			agents_[i]->computeNewVelocity(this);
+		}
+
+		for (int i = 0; i < static_cast<int>(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;
+	}
+}

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 <[email protected]>.
+ *
+ * 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
+ *
+ * <http://gamma.cs.unc.edu/RVO2/>
+ */
+
+/**
+ * \file    RVOSimulator.h
+ * \brief   Contains the RVOSimulator class.
+ */
+#ifndef RVO3D_RVO_SIMULATOR_H_
+#define RVO3D_RVO_SIMULATOR_H_
+
+#include <cstddef>
+#include <limits>
+#include <vector>
+
+#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<size_t>::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<Agent3D *> agents_;
+
+		friend class Agent3D;
+		friend class KdTree3D;
+	};
+}
+
+#endif

thirdparty/rvo2/Vector3.h → thirdparty/rvo2/rvo2_3d/Vector3.h

@@ -41,13 +41,11 @@
 #include <cstddef>
 #include <ostream>
 
-#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);
 	}