Add NavigationRegion costs for pathfinding

Add NavigationRegion costs for pathfinding.

doc/classes/NavigationRegion2D.xml

@@ -7,6 +7,9 @@
 		A region of the navigation map. It tells the [NavigationServer2D] what can be navigated and what cannot, based on its [NavigationPolygon] resource.
 		Two regions can be connected to each other if they share a similar edge. You can set the minimum distance between two vertices required to connect two edges by using [method NavigationServer2D.map_set_edge_connection_margin].
 		[b]Note:[/b] Overlapping two regions' polygons is not enough for connecting two regions. They must share a similar edge.
+		The pathfinding cost of entering this region from another region can be controlled with the [member enter_cost] value.
+		[b]Note[/b]: This value is not added to the path cost when the start position is already inside this region.
+		The pathfinding cost of traveling distances inside this region can be controlled with the [member travel_cost] multiplier.
 	</description>
 	<tutorials>
 	</tutorials>
@@ -22,11 +25,17 @@
 		<member name="enabled" type="bool" setter="set_enabled" getter="is_enabled" default="true">
 			Determines if the [NavigationRegion2D] is enabled or disabled.
 		</member>
+		<member name="enter_cost" type="float" setter="set_enter_cost" getter="get_enter_cost" default="0.0">
+			When pathfinding enters this regions navmesh from another regions navmesh the [code]enter_cost[/code] value is added to the path distance for determining the shortest path.
+		</member>
 		<member name="layers" type="int" setter="set_layers" getter="get_layers" default="1">
 			A bitfield determining all layers the region belongs to. These layers can be checked upon when requesting a path with [method NavigationServer2D.map_get_path].
 		</member>
 		<member name="navpoly" type="NavigationPolygon" setter="set_navigation_polygon" getter="get_navigation_polygon">
 			The [NavigationPolygon] resource to use.
 		</member>
+		<member name="travel_cost" type="float" setter="set_travel_cost" getter="get_travel_cost" default="1.0">
+			When pathfinding moves inside this regions navmesh the traveled distances are multiplied with [code]travel_cost[/code] for determining the shortest path.
+		</member>
 	</members>
 </class>

doc/classes/NavigationRegion3D.xml

@@ -6,6 +6,10 @@
 	<description>
 		A region of the navigation map. It tells the [NavigationServer3D] what can be navigated and what cannot, based on its [NavigationMesh] resource.
 		Two regions can be connected to each other if they share a similar edge. You can set the minimum distance between two vertices required to connect two edges by using [method NavigationServer3D.map_set_edge_connection_margin].
+		[b]Note:[/b] Overlapping two regions' navmeshes is not enough for connecting two regions. They must share a similar edge.
+		The cost of entering this region from another region can be controlled with the [member enter_cost] value.
+		[b]Note[/b]: This value is not added to the path cost when the start position is already inside this region.
+		The cost of traveling distances inside this region can be controlled with the [member travel_cost] multiplier.
 	</description>
 	<tutorials>
 	</tutorials>
@@ -28,12 +32,18 @@
 		<member name="enabled" type="bool" setter="set_enabled" getter="is_enabled" default="true">
 			Determines if the [NavigationRegion3D] is enabled or disabled.
 		</member>
+		<member name="enter_cost" type="float" setter="set_enter_cost" getter="get_enter_cost" default="0.0">
+			When pathfinding enters this regions navmesh from another regions navmesh the [code]enter_cost[/code] value is added to the path distance for determining the shortest path.
+		</member>
 		<member name="layers" type="int" setter="set_layers" getter="get_layers" default="1">
 			A bitfield determining all layers the region belongs to. These layers can be checked upon when requesting a path with [method NavigationServer3D.map_get_path].
 		</member>
 		<member name="navmesh" type="NavigationMesh" setter="set_navigation_mesh" getter="get_navigation_mesh">
 			The [NavigationMesh] resource to use.
 		</member>
+		<member name="travel_cost" type="float" setter="set_travel_cost" getter="get_travel_cost" default="1.0">
+			When pathfinding moves inside this regions navmesh the traveled distances are multiplied with [code]travel_cost[/code] for determining the shortest path.
+		</member>
 	</members>
 	<signals>
 		<signal name="bake_finished">

doc/classes/NavigationServer2D.xml

@@ -247,6 +247,13 @@
 				Returns how many connections this [code]region[/code] has with other regions in the map.
 			</description>
 		</method>
+		<method name="region_get_enter_cost" qualifiers="const">
+			<return type="float" />
+			<argument index="0" name="region" type="RID" />
+			<description>
+				Returns the [code]enter_cost[/code] of this [code]region[/code].
+			</description>
+		</method>
 		<method name="region_get_layers" qualifiers="const">
 			<return type="int" />
 			<argument index="0" name="region" type="RID" />
@@ -261,6 +268,21 @@
 				Returns the navigation map [RID] the requested [code]region[/code] is currently assigned to.
 			</description>
 		</method>
+		<method name="region_get_travel_cost" qualifiers="const">
+			<return type="float" />
+			<argument index="0" name="region" type="RID" />
+			<description>
+				Returns the [code]travel_cost[/code] of this [code]region[/code].
+			</description>
+		</method>
+		<method name="region_set_enter_cost" qualifiers="const">
+			<return type="void" />
+			<argument index="0" name="region" type="RID" />
+			<argument index="1" name="enter_cost" type="float" />
+			<description>
+				Sets the [code]enter_cost[/code] for this [code]region[/code].
+			</description>
+		</method>
 		<method name="region_set_layers" qualifiers="const">
 			<return type="void" />
 			<argument index="0" name="region" type="RID" />
@@ -293,6 +315,14 @@
 				Sets the global transformation for the region.
 			</description>
 		</method>
+		<method name="region_set_travel_cost" qualifiers="const">
+			<return type="void" />
+			<argument index="0" name="region" type="RID" />
+			<argument index="1" name="travel_cost" type="float" />
+			<description>
+				Sets the [code]travel_cost[/code] for this [code]region[/code].
+			</description>
+		</method>
 	</methods>
 	<signals>
 		<signal name="map_changed">

doc/classes/NavigationServer3D.xml

@@ -297,6 +297,13 @@
 				Returns how many connections this [code]region[/code] has with other regions in the map.
 			</description>
 		</method>
+		<method name="region_get_enter_cost" qualifiers="const">
+			<return type="float" />
+			<argument index="0" name="region" type="RID" />
+			<description>
+				Returns the [code]enter_cost[/code] of this [code]region[/code].
+			</description>
+		</method>
 		<method name="region_get_layers" qualifiers="const">
 			<return type="int" />
 			<argument index="0" name="region" type="RID" />
@@ -311,6 +318,21 @@
 				Returns the navigation map [RID] the requested [code]region[/code] is currently assigned to.
 			</description>
 		</method>
+		<method name="region_get_travel_cost" qualifiers="const">
+			<return type="float" />
+			<argument index="0" name="region" type="RID" />
+			<description>
+				Returns the [code]travel_cost[/code] of this [code]region[/code].
+			</description>
+		</method>
+		<method name="region_set_enter_cost" qualifiers="const">
+			<return type="void" />
+			<argument index="0" name="region" type="RID" />
+			<argument index="1" name="enter_cost" type="float" />
+			<description>
+				Sets the [code]enter_cost[/code] for this [code]region[/code].
+			</description>
+		</method>
 		<method name="region_set_layers" qualifiers="const">
 			<return type="void" />
 			<argument index="0" name="region" type="RID" />
@@ -343,6 +365,14 @@
 				Sets the global transformation for the region.
 			</description>
 		</method>
+		<method name="region_set_travel_cost" qualifiers="const">
+			<return type="void" />
+			<argument index="0" name="region" type="RID" />
+			<argument index="1" name="travel_cost" type="float" />
+			<description>
+				Sets the [code]travel_cost[/code] for this [code]region[/code].
+			</description>
+		</method>
 		<method name="set_active" qualifiers="const">
 			<return type="void" />
 			<argument index="0" name="active" type="bool" />

modules/navigation/godot_navigation_server.cpp

@@ -309,6 +309,36 @@ COMMAND_2(region_set_transform, RID, p_region, Transform3D, p_transform) {
 	region->set_transform(p_transform);
 }
 
+COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost) {
+	NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_COND(region == nullptr);
+	ERR_FAIL_COND(p_enter_cost < 0.0);
+
+	region->set_enter_cost(p_enter_cost);
+}
+
+real_t GodotNavigationServer::region_get_enter_cost(RID p_region) const {
+	NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_COND_V(region == nullptr, 0);
+
+	return region->get_enter_cost();
+}
+
+COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost) {
+	NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_COND(region == nullptr);
+	ERR_FAIL_COND(p_travel_cost < 0.0);
+
+	region->set_travel_cost(p_travel_cost);
+}
+
+real_t GodotNavigationServer::region_get_travel_cost(RID p_region) const {
+	NavRegion *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_COND_V(region == nullptr, 0);
+
+	return region->get_travel_cost();
+}
+
 COMMAND_2(region_set_layers, RID, p_region, uint32_t, p_layers) {
 	NavRegion *region = region_owner.get_or_null(p_region);
 	ERR_FAIL_COND(region == nullptr);

modules/navigation/godot_navigation_server.h

@@ -109,6 +109,12 @@ class GodotNavigationServer : public NavigationServer3D {
 	virtual Array map_get_agents(RID p_map) const override;
 
 	virtual RID region_create() const override;
+
+	COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost);
+	virtual real_t region_get_enter_cost(RID p_region) const override;
+	COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost);
+	virtual real_t region_get_travel_cost(RID p_region) const override;
+
 	COMMAND_2(region_set_map, RID, p_region, RID, p_map);
 	virtual RID region_get_map(RID p_region) const override;
 	COMMAND_2(region_set_layers, RID, p_region, uint32_t, p_layers);

modules/navigation/nav_map.cpp

@@ -140,6 +140,8 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 	float reachable_d = 1e30;
 	bool is_reachable = true;
 
+	gd::NavigationPoly *prev_least_cost_poly = nullptr;
+
 	while (true) {
 		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
 		for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
@@ -156,9 +158,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();
+
+				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();
+				}
+				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) + least_cost_poly->traveled_distance;
+				const float new_distance = (least_cost_poly->entry.distance_to(new_entry) * region_travel_cost) + region_enter_cost + least_cost_poly->traveled_distance;
 
 				const std::vector<gd::NavigationPoly>::iterator it = std::find(
 						navigation_polys.begin(),
@@ -238,7 +248,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 		for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
 			gd::NavigationPoly *np = &navigation_polys[element->get()];
 			float cost = np->traveled_distance;
-			cost += np->entry.distance_to(end_point);
+			cost += (np->entry.distance_to(end_point) * np->poly->owner->get_travel_cost());
 			if (cost < least_cost) {
 				least_cost_id = np->self_id;
 				least_cost = cost;
@@ -249,7 +259,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 
 		// Stores the further reachable end polygon, in case our goal is not reachable.
 		if (is_reachable) {
-			float d = navigation_polys[least_cost_id].entry.distance_to(p_destination);
+			float d = navigation_polys[least_cost_id].entry.distance_to(p_destination) * navigation_polys[least_cost_id].poly->owner->get_travel_cost();
 			if (reachable_d > d) {
 				reachable_d = d;
 				reachable_end = navigation_polys[least_cost_id].poly;

modules/navigation/nav_region.h

@@ -46,6 +46,8 @@ class NavRegion : public NavRid {
 	Transform3D transform;
 	Ref<NavigationMesh> mesh;
 	uint32_t layers = 1;
+	float enter_cost = 0.0;
+	float travel_cost = 1.0;
 	Vector<gd::Edge::Connection> connections;
 
 	bool polygons_dirty = true;
@@ -65,6 +67,12 @@ class NavRegion : public NavRid {
 		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_layers(uint32_t p_layers);
 	uint32_t get_layers() const;
 

scene/2d/navigation_region_2d.cpp

@@ -388,6 +388,26 @@ uint32_t NavigationRegion2D::get_layers() const {
 	return NavigationServer2D::get_singleton()->region_get_layers(region);
 }
 
+void NavigationRegion2D::set_enter_cost(real_t p_enter_cost) {
+	ERR_FAIL_COND_MSG(p_enter_cost < 0.0, "The enter_cost must be positive.");
+	enter_cost = MAX(p_enter_cost, 0.0);
+	NavigationServer2D::get_singleton()->region_set_enter_cost(region, p_enter_cost);
+}
+
+real_t NavigationRegion2D::get_enter_cost() const {
+	return enter_cost;
+}
+
+void NavigationRegion2D::set_travel_cost(real_t p_travel_cost) {
+	ERR_FAIL_COND_MSG(p_travel_cost < 0.0, "The travel_cost must be positive.");
+	travel_cost = MAX(p_travel_cost, 0.0);
+	NavigationServer2D::get_singleton()->region_set_enter_cost(region, travel_cost);
+}
+
+real_t NavigationRegion2D::get_travel_cost() const {
+	return travel_cost;
+}
+
 RID NavigationRegion2D::get_region_rid() const {
 	return region;
 }
@@ -544,16 +564,26 @@ void NavigationRegion2D::_bind_methods() {
 
 	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);
+	ClassDB::bind_method(D_METHOD("get_enter_cost"), &NavigationRegion2D::get_enter_cost);
+
+	ClassDB::bind_method(D_METHOD("set_travel_cost", "travel_cost"), &NavigationRegion2D::set_travel_cost);
+	ClassDB::bind_method(D_METHOD("get_travel_cost"), &NavigationRegion2D::get_travel_cost);
+
 	ClassDB::bind_method(D_METHOD("_navpoly_changed"), &NavigationRegion2D::_navpoly_changed);
 
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "navpoly", PROPERTY_HINT_RESOURCE_TYPE, "NavigationPolygon"), "set_navigation_polygon", "get_navigation_polygon");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "layers", PROPERTY_HINT_LAYERS_2D_NAVIGATION), "set_layers", "get_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");
 }
 
 NavigationRegion2D::NavigationRegion2D() {
 	set_notify_transform(true);
 	region = NavigationServer2D::get_singleton()->region_create();
+	NavigationServer2D::get_singleton()->region_set_enter_cost(region, get_enter_cost());
+	NavigationServer2D::get_singleton()->region_set_travel_cost(region, get_travel_cost());
 }
 
 NavigationRegion2D::~NavigationRegion2D() {

scene/2d/navigation_region_2d.h

@@ -98,6 +98,9 @@ class NavigationRegion2D : public Node2D {
 	RID region;
 	Ref<NavigationPolygon> navpoly;
 
+	real_t enter_cost = 0.0;
+	real_t travel_cost = 1.0;
+
 	void _navpoly_changed();
 	void _map_changed(RID p_RID);
 
@@ -119,6 +122,12 @@ class NavigationRegion2D : public Node2D {
 
 	RID get_region_rid() const;
 
+	void set_enter_cost(real_t p_enter_cost);
+	real_t get_enter_cost() const;
+
+	void set_travel_cost(real_t p_travel_cost);
+	real_t get_travel_cost() const;
+
 	void set_navigation_polygon(const Ref<NavigationPolygon> &p_navpoly);
 	Ref<NavigationPolygon> get_navigation_polygon() const;