Sara/behaviour-tree-test
1
0
Fork 0
Code Issues Pull requests Projects Releases 2 Packages Wiki Activity Actions

Add NavigationRegion costs for pathfinding

Browse source 
Add NavigationRegion costs for pathfinding.
This commit is contained in:
smix8 2022-06-06 05:24:11 +02:00
parent 25908c17c9
commit cfdfd304f1
16 changed files with 244 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

30
modules/navigation/godot_navigation_server.cpp
View file

@ -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);

6
modules/navigation/godot_navigation_server.h
View file

@ -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);

16
modules/navigation/nav_map.cpp
View file

@ -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;

8
modules/navigation/nav_region.h
View file

@ -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;