Skip to content

Commit f4e929e

Browse files
RepiteoRepiteo
committed
Merge pull request #104002 from smix8/navowner_check
Move navmesh connection owner check to subfunction
2 parents 523b7fe + fe1462c commit f4e929e

File tree

2 files changed

+71
-60
lines changed

2 files changed

+71
-60
lines changed

‎modules/navigation/3d/nav_mesh_queries_3d.cpp‎

Lines changed: 70 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -282,7 +282,6 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask
282282

283283
void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task) {
284284
const Vector3 p_target_position = p_query_task.target_position;
285-
const uint32_t p_navigation_layers = p_query_task.navigation_layers;
286285
const Polygon *begin_poly = p_query_task.begin_polygon;
287286
const Polygon *end_poly = p_query_task.end_polygon;
288287
Vector3 begin_point = p_query_task.begin_position;
@@ -325,68 +324,35 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
325324
for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
326325
const Edge::Connection &connection = edge.connections[connection_index];
327326

328-
// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
329-
const NavBaseIteration3D *owner = connection.polygon->owner;
330-
bool skip_connection = false;
331-
if (p_query_task.exclude_regions || p_query_task.include_regions) {
332-
switch (owner->get_type()) {
333-
case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION: {
334-
if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(owner->get_self())) {
335-
skip_connection = true;
336-
} else if (p_query_task.include_regions && !p_query_task.included_regions.has(owner->get_self())) {
337-
skip_connection = true;
338-
}
339-
} break;
340-
case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: {
341-
const LocalVector<Polygon> &link_polygons = owner->get_navmesh_polygons();
342-
if (link_polygons.size() != 2) {
343-
// Whatever this is, it is not a valid connected link.
344-
skip_connection = true;
345-
} else {
346-
const RID link_start_region = link_polygons[0].owner->get_self();
347-
const RID link_end_region = link_polygons[1].owner->get_self();
348-
if (p_query_task.exclude_regions && (p_query_task.excluded_regions.has(link_start_region) || p_query_task.excluded_regions.has(link_end_region))) {
349-
// At least one region of the link is excluded so skip.
350-
skip_connection = true;
351-
}
352-
if (p_query_task.include_regions && (!p_query_task.included_regions.has(link_start_region) || !p_query_task.excluded_regions.has(link_end_region))) {
353-
// Not both regions of the link are included so skip.
354-
skip_connection = true;
355-
}
356-
}
357-
} break;
358-
}
359-
}
360-
361-
if (skip_connection) {
327+
const NavBaseIteration3D *connection_owner = connection.polygon->owner;
328+
const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner);
329+
if (!owner_is_usable) {
362330
continue;
363331
}
364332

365-
if ((p_navigation_layers & owner->get_navigation_layers()) != 0) {
366-
Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
367-
const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
368-
const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance;
369-
370-
// Check if the neighbor polygon has already been processed.
371-
NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
372-
if (new_traveled_distance < neighbor_poly.traveled_distance) {
373-
// Add the polygon to the heap of polygons to traverse next.
374-
neighbor_poly.back_navigation_poly_id = least_cost_id;
375-
neighbor_poly.back_navigation_edge = connection.edge;
376-
neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
377-
neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
378-
neighbor_poly.traveled_distance = new_traveled_distance;
379-
neighbor_poly.distance_to_destination =
380-
new_entry.distance_to(end_point) *
381-
owner->get_travel_cost();
382-
neighbor_poly.entry = new_entry;
383-
384-
if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
385-
traversable_polys.shift(neighbor_poly.traversable_poly_index);
386-
} else {
387-
neighbor_poly.poly = connection.polygon;
388-
traversable_polys.push(&neighbor_poly);
389-
}
333+
Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
334+
const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
335+
const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance;
336+
337+
// Check if the neighbor polygon has already been processed.
338+
NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
339+
if (new_traveled_distance < neighbor_poly.traveled_distance) {
340+
// Add the polygon to the heap of polygons to traverse next.
341+
neighbor_poly.back_navigation_poly_id = least_cost_id;
342+
neighbor_poly.back_navigation_edge = connection.edge;
343+
neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
344+
neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
345+
neighbor_poly.traveled_distance = new_traveled_distance;
346+
neighbor_poly.distance_to_destination =
347+
new_entry.distance_to(end_point) *
348+
connection_owner->get_travel_cost();
349+
neighbor_poly.entry = new_entry;
350+
351+
if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
352+
traversable_polys.shift(neighbor_poly.traversable_poly_index);
353+
} else {
354+
neighbor_poly.poly = connection.polygon;
355+
traversable_polys.push(&neighbor_poly);
390356
}
391357
}
392358
}
@@ -1156,6 +1122,50 @@ void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_tas
11561122
}
11571123
}
11581124

1125+
bool NavMeshQueries3D::_query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner) {
1126+
bool owner_usable = true;
1127+
1128+
if ((p_query_task.navigation_layers & p_owner->get_navigation_layers()) == 0) {
1129+
// Not usable. No matching bit between task filter bitmask and owner bitmask.
1130+
owner_usable = false;
1131+
return owner_usable;
1132+
}
1133+
1134+
if (p_query_task.exclude_regions || p_query_task.include_regions) {
1135+
switch (p_owner->get_type()) {
1136+
case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION: {
1137+
if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(p_owner->get_self())) {
1138+
// Not usable. Exclude region filter is active and this region is excluded.
1139+
owner_usable = false;
1140+
} else if (p_query_task.include_regions && !p_query_task.included_regions.has(p_owner->get_self())) {
1141+
// Not usable. Include region filter is active and this region is not included.
1142+
owner_usable = false;
1143+
}
1144+
} break;
1145+
case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: {
1146+
const LocalVector<Polygon> &link_polygons = p_owner->get_navmesh_polygons();
1147+
if (link_polygons.size() != 2) {
1148+
// Not usable. Whatever this is, it is not a valid connected link.
1149+
owner_usable = false;
1150+
} else {
1151+
const RID link_start_region = link_polygons[0].owner->get_self();
1152+
const RID link_end_region = link_polygons[1].owner->get_self();
1153+
if (p_query_task.exclude_regions && (p_query_task.excluded_regions.has(link_start_region) || p_query_task.excluded_regions.has(link_end_region))) {
1154+
// Not usable. Exclude region filter is active and at least one region of the link is excluded.
1155+
owner_usable = false;
1156+
}
1157+
if (p_query_task.include_regions && (!p_query_task.included_regions.has(link_start_region) || !p_query_task.excluded_regions.has(link_end_region))) {
1158+
// Not usable. Include region filter is active and not both regions of the links are included.
1159+
owner_usable = false;
1160+
}
1161+
}
1162+
} break;
1163+
}
1164+
}
1165+
1166+
return owner_usable;
1167+
}
1168+
11591169
LocalVector<uint32_t> NavMeshQueries3D::get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon) {
11601170
p_epsilon = MAX(0.0, p_epsilon);
11611171
real_t squared_epsilon = p_epsilon * p_epsilon;

‎modules/navigation/3d/nav_mesh_queries_3d.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -141,6 +141,7 @@ class NavMeshQueries3D {
141141
static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task);
142142
static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const nav_3d::NavigationPoly *from_poly, const Vector3 &p_to_point, const nav_3d::NavigationPoly *p_to_poly);
143143
static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
144+
static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner);
144145

145146
static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
146147
static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);

0 commit comments

Comments
 (0)