Added node for Navigation links

10 files changed, 568 insertions, 46 deletions

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index 4191e46f62..9e5d666a51 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -210,6 +210,20 @@ real_t GodotNavigationServer::map_get_edge_connection_margin(RID p_map) const {
 	return map->get_edge_connection_margin();
 }
 
+COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius) {
+	NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND(map == nullptr);
+
+	map->set_link_connection_radius(p_connection_radius);
+}
+
+real_t GodotNavigationServer::map_get_link_connection_radius(RID p_map) const {
+	const NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND_V(map == nullptr, 0);
+
+	return map->get_link_connection_radius();
+}
+
 Vector<Vector3> GodotNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
 	const NavMap *map = map_owner.get_or_null(p_map);
 	ERR_FAIL_COND_V(map == nullptr, Vector<Vector3>());
@@ -245,6 +259,20 @@ RID GodotNavigationServer::map_get_closest_point_owner(RID p_map, const Vector3
 	return map->get_closest_point_owner(p_point);
 }
 
+TypedArray<RID> GodotNavigationServer::map_get_links(RID p_map) const {
+	TypedArray<RID> link_rids;
+	const NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND_V(map == nullptr, link_rids);
+
+	const LocalVector<NavLink *> links = map->get_links();
+	link_rids.resize(links.size());
+
+	for (uint32_t i = 0; i < links.size(); i++) {
+		link_rids[i] = links[i]->get_self();
+	}
+	return link_rids;
+}
+
 TypedArray<RID> GodotNavigationServer::map_get_regions(RID p_map) const {
 	TypedArray<RID> regions_rids;
 	const NavMap *map = map_owner.get_or_null(p_map);
@@ -417,6 +445,131 @@ Vector3 GodotNavigationServer::region_get_connection_pathway_end(RID p_region, i
 	return region->get_connection_pathway_end(p_connection_id);
 }
 
+RID GodotNavigationServer::link_create() const {
+	GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+	MutexLock lock(mut_this->operations_mutex);
+	RID rid = link_owner.make_rid();
+	NavLink *link = link_owner.get_or_null(rid);
+	link->set_self(rid);
+	return rid;
+}
+
+COMMAND_2(link_set_map, RID, p_link, RID, p_map) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	if (link->get_map() != nullptr) {
+		if (link->get_map()->get_self() == p_map) {
+			return; // Pointless
+		}
+
+		link->get_map()->remove_link(link);
+		link->set_map(nullptr);
+	}
+
+	if (p_map.is_valid()) {
+		NavMap *map = map_owner.get_or_null(p_map);
+		ERR_FAIL_COND(map == nullptr);
+
+		map->add_link(link);
+		link->set_map(map);
+	}
+}
+
+RID GodotNavigationServer::link_get_map(const RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, RID());
+
+	if (link->get_map()) {
+		return link->get_map()->get_self();
+	}
+	return RID();
+}
+
+COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	link->set_bidirectional(p_bidirectional);
+}
+
+bool GodotNavigationServer::link_is_bidirectional(RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, false);
+
+	return link->is_bidirectional();
+}
+
+COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	link->set_navigation_layers(p_navigation_layers);
+}
+
+uint32_t GodotNavigationServer::link_get_navigation_layers(const RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, 0);
+
+	return link->get_navigation_layers();
+}
+
+COMMAND_2(link_set_start_location, RID, p_link, Vector3, p_location) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	link->set_start_location(p_location);
+}
+
+Vector3 GodotNavigationServer::link_get_start_location(RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, Vector3());
+
+	return link->get_start_location();
+}
+
+COMMAND_2(link_set_end_location, RID, p_link, Vector3, p_location) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	link->set_end_location(p_location);
+}
+
+Vector3 GodotNavigationServer::link_get_end_location(RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, Vector3());
+
+	return link->get_end_location();
+}
+
+COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	link->set_enter_cost(p_enter_cost);
+}
+
+real_t GodotNavigationServer::link_get_enter_cost(const RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, 0);
+
+	return link->get_enter_cost();
+}
+
+COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost) {
+	NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND(link == nullptr);
+
+	link->set_travel_cost(p_travel_cost);
+}
+
+real_t GodotNavigationServer::link_get_travel_cost(const RID p_link) const {
+	const NavLink *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_COND_V(link == nullptr, 0);
+
+	return link->get_travel_cost();
+}
+
 RID GodotNavigationServer::agent_create() const {
 	GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
 	MutexLock lock(mut_this->operations_mutex);
@@ -549,6 +702,13 @@ COMMAND_1(free, RID, p_object) {
 			regions[i]->set_map(nullptr);
 		}
 
+		// Removes any assigned links
+		LocalVector<NavLink *> links = map->get_links();
+		for (uint32_t i = 0; i < links.size(); i++) {
+			map->remove_link(links[i]);
+			links[i]->set_map(nullptr);
+		}
+
 		// Remove any assigned agent
 		LocalVector<RvoAgent *> agents = map->get_agents();
 		for (uint32_t i = 0; i < agents.size(); i++) {
@@ -572,6 +732,17 @@ COMMAND_1(free, RID, p_object) {
 
 		region_owner.free(p_object);
 
+	} else if (link_owner.owns(p_object)) {
+		NavLink *link = link_owner.get_or_null(p_object);
+
+		// Removes this link from the map if assigned
+		if (link->get_map() != nullptr) {
+			link->get_map()->remove_link(link);
+			link->set_map(nullptr);
+		}
+
+		link_owner.free(p_object);
+
 	} else if (agent_owner.owns(p_object)) {
 		RvoAgent *agent = agent_owner.get_or_null(p_object);
 
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index 05ba46ede1..e6ef7e3bb1 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -36,6 +36,7 @@
 #include "core/templates/rid_owner.h"
 #include "servers/navigation_server_3d.h"
 
+#include "nav_link.h"
 #include "nav_map.h"
 #include "nav_region.h"
 #include "rvo_agent.h"
@@ -71,6 +72,7 @@ class GodotNavigationServer : public NavigationServer3D {
 
 	LocalVector<SetCommand *> commands;
 
+	mutable RID_Owner<NavLink> link_owner;
 	mutable RID_Owner<NavMap> map_owner;
 	mutable RID_Owner<NavRegion> region_owner;
 	mutable RID_Owner<RvoAgent> agent_owner;
@@ -100,6 +102,9 @@ public:
 	COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin);
 	virtual real_t map_get_edge_connection_margin(RID p_map) const override;
 
+	COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
+	virtual real_t map_get_link_connection_radius(RID p_map) const override;
+
 	virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override;
 
 	virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
@@ -107,6 +112,7 @@ public:
 	virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override;
 	virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const override;
 
+	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;
 
@@ -132,6 +138,22 @@ public:
 	virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
 	virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
 
+	virtual RID link_create() const override;
+	COMMAND_2(link_set_map, RID, p_link, RID, p_map);
+	virtual RID link_get_map(RID p_link) const override;
+	COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional);
+	virtual bool link_is_bidirectional(RID p_link) const override;
+	COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers);
+	virtual uint32_t link_get_navigation_layers(RID p_link) const override;
+	COMMAND_2(link_set_start_location, RID, p_link, Vector3, p_location);
+	virtual Vector3 link_get_start_location(RID p_link) const override;
+	COMMAND_2(link_set_end_location, RID, p_link, Vector3, p_location);
+	virtual Vector3 link_get_end_location(RID p_link) const override;
+	COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost);
+	virtual real_t link_get_enter_cost(RID p_link) const override;
+	COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost);
+	virtual real_t link_get_travel_cost(RID p_link) const override;
+
 	virtual RID agent_create() const override;
 	COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
 	virtual RID agent_get_map(RID p_agent) const override;
diff --git a/modules/navigation/nav_base.h b/modules/navigation/nav_base.h
new file mode 100644
index 0000000000..6dfaaf9af4
--- /dev/null
+++ b/modules/navigation/nav_base.h
@@ -0,0 +1,56 @@
+/*************************************************************************/
+/*  nav_base.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef NAV_BASE_H
+#define NAV_BASE_H
+
+#include "nav_rid.h"
+#include "nav_utils.h"
+
+class NavMap;
+
+class NavBase : public NavRid {
+protected:
+	uint32_t navigation_layers = 1;
+	float enter_cost = 0.0;
+	float travel_cost = 1.0;
+
+public:
+	void set_navigation_layers(uint32_t p_navigation_layers) { navigation_layers = p_navigation_layers; }
+	uint32_t get_navigation_layers() const { return navigation_layers; }
+
+	void set_enter_cost(float p_enter_cost) { enter_cost = MAX(p_enter_cost, 0.0); }
+	float get_enter_cost() const { return enter_cost; }
+
+	void set_travel_cost(float p_travel_cost) { travel_cost = MAX(p_travel_cost, 0.0); }
+	float get_travel_cost() const { return travel_cost; }
+};
+
+#endif // NAV_BASE_H
diff --git a/modules/navigation/nav_link.cpp b/modules/navigation/nav_link.cpp
new file mode 100644
index 0000000000..828b131ec6
--- /dev/null
+++ b/modules/navigation/nav_link.cpp
@@ -0,0 +1,60 @@
+/*************************************************************************/
+/*  nav_link.cpp                                                         */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#include "nav_link.h"
+
+#include "nav_map.h"
+
+void NavLink::set_map(NavMap *p_map) {
+	map = p_map;
+	link_dirty = true;
+}
+
+void NavLink::set_bidirectional(bool p_bidirectional) {
+	bidirectional = p_bidirectional;
+	link_dirty = true;
+}
+
+void NavLink::set_start_location(const Vector3 p_location) {
+	start_location = p_location;
+	link_dirty = true;
+}
+
+void NavLink::set_end_location(const Vector3 p_location) {
+	end_location = p_location;
+	link_dirty = true;
+}
+
+bool NavLink::check_dirty() {
+	const bool was_dirty = link_dirty;
+
+	link_dirty = false;
+	return was_dirty;
+}
diff --git a/modules/navigation/nav_link.h b/modules/navigation/nav_link.h
new file mode 100644
index 0000000000..8d57f076c0
--- /dev/null
+++ b/modules/navigation/nav_link.h
@@ -0,0 +1,69 @@
+/*************************************************************************/
+/*  nav_link.h                                                           */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
+/*                                                                       */
+/* Permission is hereby granted, free of charge, to any person obtaining */
+/* a copy of this software and associated documentation files (the       */
+/* "Software"), to deal in the Software without restriction, including   */
+/* without limitation the rights to use, copy, modify, merge, publish,   */
+/* distribute, sublicense, and/or sell copies of the Software, and to    */
+/* permit persons to whom the Software is furnished to do so, subject to */
+/* the following conditions:                                             */
+/*                                                                       */
+/* The above copyright notice and this permission notice shall be        */
+/* included in all copies or substantial portions of the Software.       */
+/*                                                                       */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
+/*************************************************************************/
+
+#ifndef NAV_LINK_H
+#define NAV_LINK_H
+
+#include "nav_base.h"
+#include "nav_utils.h"
+
+class NavLink : public NavBase {
+	NavMap *map = nullptr;
+	bool bidirectional = true;
+	Vector3 start_location = Vector3();
+	Vector3 end_location = Vector3();
+
+	bool link_dirty = true;
+
+public:
+	void set_map(NavMap *p_map);
+	NavMap *get_map() const {
+		return map;
+	}
+
+	void set_bidirectional(bool p_bidirectional);
+	bool is_bidirectional() const {
+		return bidirectional;
+	}
+
+	void set_start_location(Vector3 p_location);
+	Vector3 get_start_location() const {
+		return start_location;
+	}
+
+	void set_end_location(Vector3 p_location);
+	Vector3 get_end_location() const {
+		return end_location;
+	}
+
+	bool check_dirty();
+};
+
+#endif // NAV_LINK_H
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 49029b5513..100db9bc82 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -31,6 +31,7 @@
 #include "nav_map.h"
 
 #include "core/object/worker_thread_pool.h"
+#include "nav_link.h"
 #include "nav_region.h"
 #include "rvo_agent.h"
 #include <algorithm>
@@ -52,6 +53,11 @@ void NavMap::set_edge_connection_margin(float p_edge_connection_margin) {
 	regenerate_links = true;
 }
 
+void NavMap::set_link_connection_radius(float p_link_connection_radius) {
+	link_connection_radius = p_link_connection_radius;
+	regenerate_links = true;
+}
+
 gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
 	const int x = int(Math::floor(p_pos.x / cell_size));
 	const int y = int(Math::floor(p_pos.y / cell_size));
@@ -158,17 +164,17 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 					continue;
 				}
 
-				float region_enter_cost = 0.0;
-				float region_travel_cost = least_cost_poly->poly->owner->get_travel_cost();
+				float poly_enter_cost = 0.0;
+				float poly_travel_cost = least_cost_poly->poly->owner->get_travel_cost();
 
-				if (prev_least_cost_poly != nullptr && !(prev_least_cost_poly->poly->owner->get_self() == least_cost_poly->poly->owner->get_self())) {
-					region_enter_cost = least_cost_poly->poly->owner->get_enter_cost();
+				if (prev_least_cost_poly != nullptr && (prev_least_cost_poly->poly->owner->get_self() != least_cost_poly->poly->owner->get_self())) {
+					poly_enter_cost = least_cost_poly->poly->owner->get_enter_cost();
 				}
 				prev_least_cost_poly = least_cost_poly;
 
 				Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
 				const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, pathway);
-				const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance;
+				const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly->traveled_distance;
 
 				int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));
 
@@ -360,10 +366,15 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 		// Add mid points
 		int np_id = least_cost_id;
 		while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
-			int prev = navigation_polys[np_id].back_navigation_edge;
-			int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
-			Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
-			path.push_back(point);
+			if (navigation_polys[np_id].back_navigation_edge != -1) {
+				int prev = navigation_polys[np_id].back_navigation_edge;
+				int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
+				Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
+				path.push_back(point);
+			} else {
+				path.push_back(navigation_polys[np_id].entry);
+			}
+
 			np_id = navigation_polys[np_id].back_navigation_poly_id;
 		}
 
@@ -475,6 +486,19 @@ void NavMap::remove_region(NavRegion *p_region) {
 	}
 }
 
+void NavMap::add_link(NavLink *p_link) {
+	links.push_back(p_link);
+	regenerate_links = true;
+}
+
+void NavMap::remove_link(NavLink *p_link) {
+	int64_t link_index = links.find(p_link);
+	if (link_index != -1) {
+		links.remove_at_unordered(link_index);
+		regenerate_links = true;
+	}
+}
+
 bool NavMap::has_agent(RvoAgent *agent) const {
 	return (agents.find(agent) != -1);
 }
@@ -526,6 +550,12 @@ void NavMap::sync() {
 		}
 	}
 
+	for (uint32_t l = 0; l < links.size(); l++) {
+		if (links[l]->check_dirty()) {
+			regenerate_links = true;
+		}
+	}
+
 	if (regenerate_links) {
 		// Remove regions connections.
 		for (uint32_t r = 0; r < regions.size(); r++) {
@@ -651,7 +681,121 @@ void NavMap::sync() {
 				free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
 
 				// Add the connection to the region_connection map.
-				free_edge.polygon->owner->get_connections().push_back(new_connection);
+				((NavRegion *)free_edge.polygon->owner)->get_connections().push_back(new_connection);
+			}
+		}
+
+		uint32_t link_poly_idx = 0;
+		link_polygons.resize(links.size());
+
+		// Search for polygons within range of a nav link.
+		for (uint32_t l = 0; l < links.size(); l++) {
+			const NavLink *link = links[l];
+			const Vector3 start = link->get_start_location();
+			const Vector3 end = link->get_end_location();
+
+			gd::Polygon *closest_start_polygon = nullptr;
+			real_t closest_start_distance = link_connection_radius;
+			Vector3 closest_start_point;
+
+			gd::Polygon *closest_end_polygon = nullptr;
+			real_t closest_end_distance = link_connection_radius;
+			Vector3 closest_end_point;
+
+			// Create link to any polygons within the search radius of the start point.
+			for (uint32_t start_index = 0; start_index < polygons.size(); start_index++) {
+				gd::Polygon &start_poly = polygons[start_index];
+
+				// For each face check the distance to the start
+				for (uint32_t start_point_id = 2; start_point_id < start_poly.points.size(); start_point_id += 1) {
+					const Face3 start_face(start_poly.points[0].pos, start_poly.points[start_point_id - 1].pos, start_poly.points[start_point_id].pos);
+					const Vector3 start_point = start_face.get_closest_point_to(start);
+					const real_t start_distance = start_point.distance_to(start);
+
+					// Pick the polygon that is within our radius and is closer than anything we've seen yet.
+					if (start_distance <= link_connection_radius && start_distance < closest_start_distance) {
+						closest_start_distance = start_distance;
+						closest_start_point = start_point;
+						closest_start_polygon = &start_poly;
+					}
+				}
+			}
+
+			// Find any polygons within the search radius of the end point.
+			for (uint32_t end_index = 0; end_index < polygons.size(); end_index++) {
+				gd::Polygon &end_poly = polygons[end_index];
+
+				// For each face check the distance to the end
+				for (uint32_t end_point_id = 2; end_point_id < end_poly.points.size(); end_point_id += 1) {
+					const Face3 end_face(end_poly.points[0].pos, end_poly.points[end_point_id - 1].pos, end_poly.points[end_point_id].pos);
+					const Vector3 end_point = end_face.get_closest_point_to(end);
+					const real_t end_distance = end_point.distance_to(end);
+
+					// Pick the polygon that is within our radius and is closer than anything we've seen yet.
+					if (end_distance <= link_connection_radius && end_distance < closest_end_distance) {
+						closest_end_distance = end_distance;
+						closest_end_point = end_point;
+						closest_end_polygon = &end_poly;
+					}
+				}
+			}
+
+			// If we have both a start and end point, then create a synthetic polygon to route through.
+			if (closest_start_polygon && closest_end_polygon) {
+				gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
+				new_polygon.owner = link;
+
+				new_polygon.edges.clear();
+				new_polygon.edges.resize(4);
+				new_polygon.points.clear();
+				new_polygon.points.reserve(4);
+
+				// Build a set of vertices that create a thin polygon going from the start to the end point.
+				new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
+				new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
+				new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
+				new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
+
+				Vector3 center;
+				for (int p = 0; p < 4; ++p) {
+					center += new_polygon.points[p].pos;
+				}
+				new_polygon.center = center / real_t(new_polygon.points.size());
+				new_polygon.clockwise = true;
+
+				// Setup connections to go forward in the link.
+				{
+					gd::Edge::Connection entry_connection;
+					entry_connection.polygon = &new_polygon;
+					entry_connection.edge = -1;
+					entry_connection.pathway_start = new_polygon.points[0].pos;
+					entry_connection.pathway_end = new_polygon.points[1].pos;
+					closest_start_polygon->edges[0].connections.push_back(entry_connection);
+
+					gd::Edge::Connection exit_connection;
+					exit_connection.polygon = closest_end_polygon;
+					exit_connection.edge = -1;
+					exit_connection.pathway_start = new_polygon.points[2].pos;
+					exit_connection.pathway_end = new_polygon.points[3].pos;
+					new_polygon.edges[2].connections.push_back(exit_connection);
+				}
+
+				// If the link is bi-directional, create connections from the end to the start.
+				if (link->is_bidirectional()) {
+					gd::Edge::Connection entry_connection;
+					entry_connection.polygon = &new_polygon;
+					entry_connection.edge = -1;
+					entry_connection.pathway_start = new_polygon.points[2].pos;
+					entry_connection.pathway_end = new_polygon.points[3].pos;
+					closest_end_polygon->edges[0].connections.push_back(entry_connection);
+
+					gd::Edge::Connection exit_connection;
+					exit_connection.polygon = closest_start_polygon;
+					exit_connection.edge = -1;
+					exit_connection.pathway_start = new_polygon.points[0].pos;
+					exit_connection.pathway_end = new_polygon.points[1].pos;
+					new_polygon.edges[0].connections.push_back(exit_connection);
+				}
 			}
 		}
 
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index e50a1afbe9..a3da9fa727 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -40,9 +40,9 @@
 
 #include <KdTree.h>
 
+class NavLink;
 class NavRegion;
 class RvoAgent;
-class NavRegion;
 
 class NavMap : public NavRid {
 	/// Map Up
@@ -55,11 +55,19 @@ class NavMap : public NavRid {
 	/// This value is used to detect the near edges to connect.
 	real_t edge_connection_margin = 0.25;
 
+	/// This value is used to limit how far links search to find polygons to connect to.
+	real_t link_connection_radius = 1.0;
+
 	bool regenerate_polygons = true;
 	bool regenerate_links = true;
 
+	/// Map regions
 	LocalVector<NavRegion *> regions;
 
+	/// Map links
+	LocalVector<NavLink *> links;
+	LocalVector<gd::Polygon> link_polygons;
+
 	/// Map polygons
 	LocalVector<gd::Polygon> polygons;
 
@@ -100,6 +108,11 @@ public:
 		return edge_connection_margin;
 	}
 
+	void set_link_connection_radius(float p_link_connection_radius);
+	float get_link_connection_radius() const {
+		return link_connection_radius;
+	}
+
 	gd::PointKey get_point_key(const Vector3 &p_pos) const;
 
 	Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const;
@@ -115,6 +128,12 @@ public:
 		return regions;
 	}
 
+	void add_link(NavLink *p_link);
+	void remove_link(NavLink *p_link);
+	const LocalVector<NavLink *> &get_links() const {
+		return links;
+	}
+
 	bool has_agent(RvoAgent *agent) const;
 	void add_agent(RvoAgent *agent);
 	void remove_agent(RvoAgent *agent);
diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
index 88740807eb..d43f53d1c0 100644
--- a/modules/navigation/nav_region.cpp
+++ b/modules/navigation/nav_region.cpp
@@ -40,14 +40,6 @@ void NavRegion::set_map(NavMap *p_map) {
 	}
 }
 
-void NavRegion::set_navigation_layers(uint32_t p_navigation_layers) {
-	navigation_layers = p_navigation_layers;
-}
-
-uint32_t NavRegion::get_navigation_layers() const {
-	return navigation_layers;
-}
-
 void NavRegion::set_transform(Transform3D p_transform) {
 	transform = p_transform;
 	polygons_dirty = true;
diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
index c9d2d80f6c..8d2b5aa9eb 100644
--- a/modules/navigation/nav_region.h
+++ b/modules/navigation/nav_region.h
@@ -33,21 +33,13 @@
 
 #include "scene/resources/navigation_mesh.h"
 
-#include "nav_rid.h"
+#include "nav_base.h"
 #include "nav_utils.h"
 
-#include <vector>
-
-class NavMap;
-class NavRegion;
-
-class NavRegion : public NavRid {
+class NavRegion : public NavBase {
 	NavMap *map = nullptr;
 	Transform3D transform;
 	Ref<NavigationMesh> mesh;
-	uint32_t navigation_layers = 1;
-	float enter_cost = 0.0;
-	float travel_cost = 1.0;
 	Vector<gd::Edge::Connection> connections;
 
 	bool polygons_dirty = true;
@@ -67,15 +59,6 @@ public:
 		return map;
 	}
 
-	void set_enter_cost(float p_enter_cost) { enter_cost = MAX(p_enter_cost, 0.0); }
-	float get_enter_cost() const { return enter_cost; }
-
-	void set_travel_cost(float p_travel_cost) { travel_cost = MAX(p_travel_cost, 0.0); }
-	float get_travel_cost() const { return travel_cost; }
-
-	void set_navigation_layers(uint32_t p_navigation_layers);
-	uint32_t get_navigation_layers() const;
-
 	void set_transform(Transform3D transform);
 	const Transform3D &get_transform() const {
 		return transform;
diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h
index 47f04b6a75..16b96dcfe9 100644
--- a/modules/navigation/nav_utils.h
+++ b/modules/navigation/nav_utils.h
@@ -35,9 +35,8 @@
 #include "core/templates/hash_map.h"
 #include "core/templates/hashfuncs.h"
 #include "core/templates/local_vector.h"
-#include <vector>
 
-class NavRegion;
+class NavBase;
 
 namespace gd {
 struct Polygon;
@@ -79,26 +78,33 @@ struct Point {
 };
 
 struct Edge {
-	/// This edge ID
-	int this_edge = -1;
-
 	/// The gateway in the edge, as, in some case, the whole edge might not be navigable.
 	struct Connection {
+		/// Polygon that this connection leads to.
 		Polygon *polygon = nullptr;
+
+		/// Edge of the source polygon where this connection starts from.
 		int edge = -1;
+
+		/// Point on the edge where the gateway leading to the poly starts.
 		Vector3 pathway_start;
+
+		/// Point on the edge where the gateway leading to the poly ends.
 		Vector3 pathway_end;
 	};
+
+	/// Connections from this edge to other polygons.
 	Vector<Connection> connections;
 };
 
 struct Polygon {
-	NavRegion *owner = nullptr;
+	/// Navigation region or link that contains this polygon.
+	const NavBase *owner = nullptr;
 
 	/// The points of this `Polygon`
 	LocalVector<Point> points;
 
-	/// Are the points clockwise ?
+	/// Are the points clockwise?
 	bool clockwise;
 
 	/// The edges of this `Polygon`
@@ -115,7 +121,7 @@ struct NavigationPoly {
 
 	/// Those 4 variables are used to travel the path backwards.
 	int back_navigation_poly_id = -1;
-	uint32_t back_navigation_edge = UINT32_MAX;
+	int back_navigation_edge = -1;
 	Vector3 back_navigation_edge_pathway_start;
 	Vector3 back_navigation_edge_pathway_end;