Rework Navigation Avoidance

Rework Navigation Avoidance.

9 files changed, 1012 insertions, 89 deletions

diff --git a/modules/navigation/SCsub b/modules/navigation/SCsub
index a9277657f4..46bcb0fba4 100644
--- a/modules/navigation/SCsub
+++ b/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
 
diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index ee9687cf3d..63423e655c 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/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).");
 	}
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index 0b113b77d4..ee9b1f05b7 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -39,6 +39,7 @@
 #include "nav_agent.h"
 #include "nav_link.h"
 #include "nav_map.h"
+#include "nav_obstacle.h"
 #include "nav_region.h"
 
 /// The commands are functions executed during the `sync` phase.
@@ -72,6 +73,7 @@ class GodotNavigationServer : public NavigationServer3D {
 	mutable RID_Owner<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);
 
diff --git a/modules/navigation/nav_agent.cpp b/modules/navigation/nav_agent.cpp
index 293544c0a5..a0efe4c74c 100644
--- a/modules/navigation/nav_agent.cpp
+++ b/modules/navigation/nav_agent.cpp
@@ -32,8 +32,66 @@
 
 #include "nav_map.h"
 
+NavAgent::NavAgent() {
+}
+
+void NavAgent::set_avoidance_enabled(bool p_enabled) {
+	avoidance_enabled = p_enabled;
+	_update_rvo_agent_properties();
+}
+
+void NavAgent::set_use_3d_avoidance(bool p_enabled) {
+	use_3d_avoidance = p_enabled;
+	_update_rvo_agent_properties();
+}
+
+void NavAgent::_update_rvo_agent_properties() {
+	if (use_3d_avoidance) {
+		rvo_agent_3d.neighborDist_ = neighbor_distance;
+		rvo_agent_3d.maxNeighbors_ = max_neighbors;
+		rvo_agent_3d.timeHorizon_ = time_horizon_agents;
+		rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
+		rvo_agent_3d.radius_ = radius;
+		rvo_agent_3d.maxSpeed_ = max_speed;
+		rvo_agent_3d.position_ = RVO3D::Vector3(position.x, position.y, position.z);
+		// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
+		//rvo_agent_3d.velocity_ = RVO3D::Vector3(velocity.x, velocity.y ,velocity.z);
+		rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
+		rvo_agent_3d.height_ = height;
+		rvo_agent_3d.avoidance_layers_ = avoidance_layers;
+		rvo_agent_3d.avoidance_mask_ = avoidance_mask;
+		rvo_agent_3d.avoidance_priority_ = avoidance_priority;
+	} else {
+		rvo_agent_2d.neighborDist_ = neighbor_distance;
+		rvo_agent_2d.maxNeighbors_ = max_neighbors;
+		rvo_agent_2d.timeHorizon_ = time_horizon_agents;
+		rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
+		rvo_agent_2d.radius_ = radius;
+		rvo_agent_2d.maxSpeed_ = max_speed;
+		rvo_agent_2d.position_ = RVO2D::Vector2(position.x, position.z);
+		rvo_agent_2d.elevation_ = position.y;
+		// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
+		//rvo_agent_2d.velocity_ = RVO2D::Vector2(velocity.x, velocity.z);
+		rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
+		rvo_agent_2d.height_ = height;
+		rvo_agent_2d.avoidance_layers_ = avoidance_layers;
+		rvo_agent_2d.avoidance_mask_ = avoidance_mask;
+		rvo_agent_2d.avoidance_priority_ = avoidance_priority;
+	}
+
+	if (map != nullptr) {
+		if (avoidance_enabled) {
+			map->set_agent_as_controlled(this);
+		} else {
+			map->remove_agent_as_controlled(this);
+		}
+	}
+	agent_dirty = true;
+}
+
 void NavAgent::set_map(NavMap *p_map) {
 	map = p_map;
+	agent_dirty = true;
 }
 
 bool NavAgent::is_map_changed() {
@@ -46,25 +104,241 @@ bool NavAgent::is_map_changed() {
 	}
 }
 
-void NavAgent::set_callback(Callable p_callback) {
-	callback = p_callback;
+void NavAgent::set_avoidance_callback(Callable p_callback) {
+	avoidance_callback = p_callback;
 }
 
-bool NavAgent::has_callback() const {
-	return callback.is_valid();
+bool NavAgent::has_avoidance_callback() const {
+	return avoidance_callback.is_valid();
 }
 
-void NavAgent::dispatch_callback() {
-	if (!callback.is_valid()) {
+void NavAgent::dispatch_avoidance_callback() {
+	if (!avoidance_callback.is_valid()) {
 		return;
 	}
 
-	Vector3 new_velocity = Vector3(agent.newVelocity_.x(), agent.newVelocity_.y(), agent.newVelocity_.z());
+	Vector3 new_velocity;
+
+	if (use_3d_avoidance) {
+		new_velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
+	} else {
+		new_velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
+	}
+
+	if (clamp_speed) {
+		new_velocity = new_velocity.limit_length(max_speed);
+	}
 
 	// Invoke the callback with the new velocity.
 	Variant args[] = { new_velocity };
 	const Variant *args_p[] = { &args[0] };
 	Variant return_value;
 	Callable::CallError call_error;
-	callback.callp(args_p, 1, return_value, call_error);
+
+	avoidance_callback.callp(args_p, 1, return_value, call_error);
+}
+
+void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) {
+	neighbor_distance = p_neighbor_distance;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.neighborDist_ = neighbor_distance;
+	} else {
+		rvo_agent_2d.neighborDist_ = neighbor_distance;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_max_neighbors(int p_max_neighbors) {
+	max_neighbors = p_max_neighbors;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.maxNeighbors_ = max_neighbors;
+	} else {
+		rvo_agent_2d.maxNeighbors_ = max_neighbors;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
+	time_horizon_agents = p_time_horizon;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.timeHorizon_ = time_horizon_agents;
+	} else {
+		rvo_agent_2d.timeHorizon_ = time_horizon_agents;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
+	time_horizon_obstacles = p_time_horizon;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
+	} else {
+		rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_radius(real_t p_radius) {
+	radius = p_radius;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.radius_ = radius;
+	} else {
+		rvo_agent_2d.radius_ = radius;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_height(real_t p_height) {
+	height = p_height;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.height_ = height;
+	} else {
+		rvo_agent_2d.height_ = height;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_max_speed(real_t p_max_speed) {
+	max_speed = p_max_speed;
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.maxSpeed_ = max_speed;
+		} else {
+			rvo_agent_2d.maxSpeed_ = max_speed;
+		}
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_position(const Vector3 p_position) {
+	position = p_position;
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.position_ = RVO3D::Vector3(p_position.x, p_position.y, p_position.z);
+		} else {
+			rvo_agent_2d.elevation_ = p_position.y;
+			rvo_agent_2d.position_ = RVO2D::Vector2(p_position.x, p_position.z);
+		}
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_target_position(const Vector3 p_target_position) {
+	target_position = p_target_position;
+}
+
+void NavAgent::set_velocity(const Vector3 p_velocity) {
+	// Sets the "wanted" velocity for an agent as a suggestion
+	// This velocity is not guaranteed, RVO simulation will only try to fulfill it
+	velocity = p_velocity;
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
+		} else {
+			rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
+		}
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
+	// This function replaces the internal rvo simulation velocity
+	// should only be used after the agent was teleported
+	// as it destroys consistency in movement in cramped situations
+	// use velocity instead to update with a safer "suggestion"
+	velocity_forced = p_velocity;
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.velocity_ = RVO3D::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+		} else {
+			rvo_agent_2d.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.z);
+		}
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::update() {
+	// Updates this agent with the calculated results from the rvo simulation
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
+		} else {
+			velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
+		}
+	}
+}
+
+void NavAgent::set_avoidance_mask(uint32_t p_mask) {
+	avoidance_mask = p_mask;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.avoidance_mask_ = avoidance_mask;
+	} else {
+		rvo_agent_2d.avoidance_mask_ = avoidance_mask;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_avoidance_layers(uint32_t p_layers) {
+	avoidance_layers = p_layers;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.avoidance_layers_ = avoidance_layers;
+	} else {
+		rvo_agent_2d.avoidance_layers_ = avoidance_layers;
+	}
+	agent_dirty = true;
+}
+
+void NavAgent::set_avoidance_priority(real_t p_priority) {
+	ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	avoidance_priority = p_priority;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.avoidance_priority_ = avoidance_priority;
+	} else {
+		rvo_agent_2d.avoidance_priority_ = avoidance_priority;
+	}
+	agent_dirty = true;
+};
+
+bool NavAgent::check_dirty() {
+	const bool was_dirty = agent_dirty;
+	agent_dirty = false;
+	return was_dirty;
+}
+
+const Dictionary NavAgent::get_avoidance_data() const {
+	// Returns debug data from RVO simulation internals of this agent.
+	Dictionary _avoidance_data;
+	if (use_3d_avoidance) {
+		_avoidance_data["max_neighbors"] = int(rvo_agent_3d.maxNeighbors_);
+		_avoidance_data["max_speed"] = float(rvo_agent_3d.maxSpeed_);
+		_avoidance_data["neighbor_distance"] = float(rvo_agent_3d.neighborDist_);
+		_avoidance_data["new_velocity"] = Vector3(rvo_agent_3d.newVelocity_.x(), rvo_agent_3d.newVelocity_.y(), rvo_agent_3d.newVelocity_.z());
+		_avoidance_data["velocity"] = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
+		_avoidance_data["position"] = Vector3(rvo_agent_3d.position_.x(), rvo_agent_3d.position_.y(), rvo_agent_3d.position_.z());
+		_avoidance_data["prefered_velocity"] = Vector3(rvo_agent_3d.prefVelocity_.x(), rvo_agent_3d.prefVelocity_.y(), rvo_agent_3d.prefVelocity_.z());
+		_avoidance_data["radius"] = float(rvo_agent_3d.radius_);
+		_avoidance_data["time_horizon_agents"] = float(rvo_agent_3d.timeHorizon_);
+		_avoidance_data["time_horizon_obstacles"] = 0.0;
+		_avoidance_data["height"] = float(rvo_agent_3d.height_);
+		_avoidance_data["avoidance_layers"] = int(rvo_agent_3d.avoidance_layers_);
+		_avoidance_data["avoidance_mask"] = int(rvo_agent_3d.avoidance_mask_);
+		_avoidance_data["avoidance_priority"] = float(rvo_agent_3d.avoidance_priority_);
+	} else {
+		_avoidance_data["max_neighbors"] = int(rvo_agent_2d.maxNeighbors_);
+		_avoidance_data["max_speed"] = float(rvo_agent_2d.maxSpeed_);
+		_avoidance_data["neighbor_distance"] = float(rvo_agent_2d.neighborDist_);
+		_avoidance_data["new_velocity"] = Vector3(rvo_agent_2d.newVelocity_.x(), 0.0, rvo_agent_2d.newVelocity_.y());
+		_avoidance_data["velocity"] = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
+		_avoidance_data["position"] = Vector3(rvo_agent_2d.position_.x(), 0.0, rvo_agent_2d.position_.y());
+		_avoidance_data["prefered_velocity"] = Vector3(rvo_agent_2d.prefVelocity_.x(), 0.0, rvo_agent_2d.prefVelocity_.y());
+		_avoidance_data["radius"] = float(rvo_agent_2d.radius_);
+		_avoidance_data["time_horizon_agents"] = float(rvo_agent_2d.timeHorizon_);
+		_avoidance_data["time_horizon_obstacles"] = float(rvo_agent_2d.timeHorizonObst_);
+		_avoidance_data["height"] = float(rvo_agent_2d.height_);
+		_avoidance_data["avoidance_layers"] = int(rvo_agent_2d.avoidance_layers_);
+		_avoidance_data["avoidance_mask"] = int(rvo_agent_2d.avoidance_mask_);
+		_avoidance_data["avoidance_priority"] = float(rvo_agent_2d.avoidance_priority_);
+	}
+	return _avoidance_data;
 }
diff --git a/modules/navigation/nav_agent.h b/modules/navigation/nav_agent.h
index f154ce14d9..497b239f84 100644
--- a/modules/navigation/nav_agent.h
+++ b/modules/navigation/nav_agent.h
@@ -32,34 +32,121 @@
 #define NAV_AGENT_H
 
 #include "core/object/class_db.h"
+#include "core/templates/local_vector.h"
+#include "nav_agent.h"
 #include "nav_rid.h"
 
-#include <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
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 91b13ba9c4..b76295db86 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -30,11 +30,14 @@
 
 #include "nav_map.h"
 
+#include "core/config/project_settings.h"
 #include "core/object/worker_thread_pool.h"
 #include "nav_agent.h"
 #include "nav_link.h"
+#include "nav_obstacle.h"
 #include "nav_region.h"
-#include <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() {
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index 5ec2c2826c..343f53760b 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -38,11 +38,16 @@
 #include "core/templates/rb_map.h"
 #include "nav_utils.h"
 
-#include <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
diff --git a/modules/navigation/nav_obstacle.cpp b/modules/navigation/nav_obstacle.cpp
new file mode 100644
index 0000000000..5d0bc59cbb
--- /dev/null
+++ b/modules/navigation/nav_obstacle.cpp
@@ -0,0 +1,86 @@
+/**************************************************************************/
+/*  nav_obstacle.cpp                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_obstacle.h"
+
+#include "nav_map.h"
+
+NavObstacle::NavObstacle() {}
+
+NavObstacle::~NavObstacle() {}
+
+void NavObstacle::set_map(NavMap *p_map) {
+	if (map != p_map) {
+		map = p_map;
+		obstacle_dirty = true;
+	}
+}
+
+void NavObstacle::set_position(const Vector3 p_position) {
+	if (position != p_position) {
+		position = p_position;
+		obstacle_dirty = true;
+	}
+}
+
+void NavObstacle::set_height(const real_t p_height) {
+	if (height != p_height) {
+		height = p_height;
+		obstacle_dirty = true;
+	}
+}
+
+void NavObstacle::set_vertices(const Vector<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;
+}
diff --git a/modules/navigation/nav_obstacle.h b/modules/navigation/nav_obstacle.h
new file mode 100644
index 0000000000..f59eba5200
--- /dev/null
+++ b/modules/navigation/nav_obstacle.h
@@ -0,0 +1,78 @@
+/**************************************************************************/
+/*  nav_obstacle.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#ifndef NAV_OBSTACLE_H
+#define NAV_OBSTACLE_H
+
+#include "core/object/class_db.h"
+#include "core/templates/local_vector.h"
+#include "nav_agent.h"
+#include "nav_rid.h"
+
+class NavMap;
+
+class NavObstacle : public NavRid {
+	NavMap *map = nullptr;
+	Vector3 position;
+	Vector<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