Add NavigationRegion costs for pathfinding

Add NavigationRegion costs for pathfinding.

4 files changed, 57 insertions, 3 deletions

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index cc9d05da47..f4e719b0c1 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/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);
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index 89e7311e51..1a7fddb7e6 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -109,6 +109,12 @@ public:
 	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);
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 344475fb37..059dc989a8 100644
--- a/modules/navigation/nav_map.cpp
+++ b/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;
diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
index 7a6da281c0..acde5de834 100644
--- a/modules/navigation/nav_region.h
+++ b/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 @@ 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_layers(uint32_t p_layers);
 	uint32_t get_layers() const;