Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add NavigationRegion costs for 2D and 3D pathfinding #61739

Merged
merged 1 commit into from
Jun 8, 2022
Merged

Add NavigationRegion costs for 2D and 3D pathfinding #61739

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions doc/classes/NavigationRegion2D.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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>
10 changes: 10 additions & 0 deletions doc/classes/NavigationRegion3D.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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">
Expand Down
30 changes: 30 additions & 0 deletions doc/classes/NavigationServer2D.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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" />
Expand All @@ -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" />
Expand Down Expand Up @@ -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">
Expand Down
30 changes: 30 additions & 0 deletions doc/classes/NavigationServer3D.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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" />
Expand All @@ -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" />
Expand Down Expand Up @@ -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" />
Expand Down
30 changes: 30 additions & 0 deletions modules/navigation/godot_navigation_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
6 changes: 6 additions & 0 deletions modules/navigation/godot_navigation_server.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
16 changes: 13 additions & 3 deletions modules/navigation/nav_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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++) {
Expand All @@ -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(),
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down
8 changes: 8 additions & 0 deletions modules/navigation/nav_region.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;

Expand Down
30 changes: 30 additions & 0 deletions scene/2d/navigation_region_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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() {
Expand Down
9 changes: 9 additions & 0 deletions scene/2d/navigation_region_2d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

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

Expand Down
Loading