From cfdfd304f1ad08b0498eda97b502aaccd95f559d Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Mon, 6 Jun 2022 05:24:11 +0200 Subject: Add NavigationRegion costs for pathfinding Add NavigationRegion costs for pathfinding. --- modules/navigation/godot_navigation_server.cpp | 30 ++++++++++++++++++++++++++ modules/navigation/godot_navigation_server.h | 6 ++++++ modules/navigation/nav_map.cpp | 16 +++++++++++--- modules/navigation/nav_region.h | 8 +++++++ 4 files changed, 57 insertions(+), 3 deletions(-) (limited to 'modules/navigation') 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 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 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::iterator it = std::find( navigation_polys.begin(), @@ -238,7 +248,7 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p for (List::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 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 mesh; uint32_t layers = 1; + float enter_cost = 0.0; + float travel_cost = 1.0; Vector 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; -- cgit v1.2.3