Skip to content

Commit

Permalink
Cleanup despaghettify NavigationServer path queries
Browse files Browse the repository at this point in the history 
Removes unused function parameters, moves some to the query task, renames functions for consistency and grouping.
  • Loading branch information
@smix8
smix8 committed Dec 16, 2024
1 parent b9437c3 commit dbda4a6
Show file tree
Hide file tree
Showing 3 changed files with 90 additions and 73 deletions.
135 changes: 74 additions & 61 deletions modules/navigation/3d/nav_mesh_queries_3d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
}
}

void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *begin_poly, const gd::Polygon *end_poly) {
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
p_query_task.path_meta_point_types.resize(2);
p_query_task.path_meta_point_types[0] = begin_poly->owner->get_type();
Expand All @@ -148,11 +148,11 @@ void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPat
}

p_query_task.path_points.resize(2);
p_query_task.path_points[0] = begin_point;
p_query_task.path_points[1] = end_point;
p_query_task.path_points[0] = p_query_task.begin_point;
p_query_task.path_points[1] = p_query_task.end_point;
}

void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, Vector3 p_point, const gd::Polygon *p_point_polygon) {
void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon) {
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
}
Expand Down Expand Up @@ -267,48 +267,42 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQuery
}
}

void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size) {
void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
p_query_task.path_points.clear();
p_query_task.path_meta_point_types.clear();
p_query_task.path_meta_point_rids.clear();
p_query_task.path_meta_point_owners.clear();

// Find begin polyon and begin position closest to start position and
// end polyon and end position closest to target position on the map.
const gd::Polygon *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr;
Vector3 begin_point;
Vector3 end_point;

_query_task_find_start_end_positions(p_query_task, p_polygons, &begin_poly, begin_point, &end_poly, end_point);
_query_task_find_start_end_positions(p_query_task, p_polygons);

// Check for trivial cases
if (!begin_poly || !end_poly) {
if (!p_query_task.begin_poly || !p_query_task.end_poly) {
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED;
return;
}

if (begin_poly == end_poly) {
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
if (p_query_task.begin_poly == p_query_task.end_poly) {
_query_task_create_same_polygon_two_point_path(p_query_task, p_query_task.begin_poly, p_query_task.end_poly);
return;
}

_query_task_build_path_corridor(p_query_task, p_polygons, p_map_up, p_link_polygons_size, begin_poly, begin_point, end_poly, end_point);
DEV_ASSERT(p_query_task.path_query_slot->path_corridor.size() == p_polygons.size() + p_query_task.link_polygons_size);
_query_task_build_path_corridor(p_query_task, p_polygons);

// Post-Process path.
switch (p_query_task.path_postprocessing) {
case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
_query_task_post_process_corridorfunnel(p_query_task);
} break;
case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
_path_corridor_post_process_edgecentered(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
_query_task_post_process_edgecentered(p_query_task);
} break;
case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
_path_corridor_post_process_nopostprocessing(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point);
_query_task_post_process_nopostprocessing(p_query_task);
} break;
default: {
WARN_PRINT("No match for used PathPostProcessing - fallback to default");
_path_corridor_post_process_corridorfunnel(p_query_task, p_query_task.least_cost_id, begin_poly, begin_point, end_poly, end_point, p_map_up);
_query_task_post_process_corridorfunnel(p_query_task);
} break;
}

Expand Down Expand Up @@ -339,15 +333,17 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
}

void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_map_up, uint32_t p_link_polygons_size, const gd::Polygon *begin_poly, Vector3 begin_point, const gd::Polygon *end_poly, Vector3 end_point) {
void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
const gd::Polygon *begin_poly = p_query_task.begin_poly;
const gd::Polygon *end_poly = p_query_task.end_poly;
const Vector3 &begin_point = p_query_task.begin_point;
Vector3 &end_point = p_query_task.end_point;
// List of all reachable navigation polys.
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
for (gd::NavigationPoly &polygon : navigation_polys) {
polygon.reset();
}

DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_link_polygons_size);

// Initialize the matching navigation polygon.
gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
begin_navigation_poly.poly = begin_poly;
Expand Down Expand Up @@ -472,7 +468,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
}

if (closest_point_on_start_poly) {
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, end_poly, end_point);
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, end_poly);
return;
}

Expand Down Expand Up @@ -522,7 +518,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
end_d = dpoint;
}
}
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_point, begin_poly, end_point);
_query_task_create_same_polygon_two_point_path(p_query_task, begin_poly, begin_poly);
return;
}
}
Expand Down Expand Up @@ -569,23 +565,26 @@ void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D
}
}

void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point, const Vector3 &p_map_up) {
void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task) {
const Vector3 &begin_point = p_query_task.begin_point;
const Vector3 &end_point = p_query_task.end_point;
const Vector3 &map_up = p_query_task.map_up;
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;

// Set the apex poly/point to the end point
gd::NavigationPoly *apex_poly = &p_path_corridor[p_least_cost_id];
gd::NavigationPoly *apex_poly = &p_path_corridor[p_query_task.least_cost_id];

Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(p_end_point, back_pathway);
if (p_end_point.is_equal_approx(back_edge_closest_point)) {
const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
if (end_point.is_equal_approx(back_edge_closest_point)) {
// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
if (apex_poly->back_navigation_poly_id != -1) {
apex_poly = &p_path_corridor[apex_poly->back_navigation_poly_id];
}
}

Vector3 apex_point = p_end_point;
Vector3 apex_point = end_point;

gd::NavigationPoly *left_poly = apex_poly;
Vector3 left_portal = apex_point;
Expand All @@ -594,24 +593,24 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue

gd::NavigationPoly *p = apex_poly;

_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
_query_task_push_back_point_with_metadata(p_query_task, end_point, p_query_task.end_poly);

while (p) {
// Set left and right points of the pathway between polygons.
Vector3 left = p->back_navigation_edge_pathway_start;
Vector3 right = p->back_navigation_edge_pathway_end;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(p_map_up) < 0) {
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(map_up) < 0) {
SWAP(left, right);
}

bool skip = false;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) {
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(map_up) >= 0) {
//process
if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) {
if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(map_up) > 0) {
left_poly = p;
left_portal = left;
} else {
clip_path(p_query_task, p_path_corridor, apex_poly, right_portal, right_poly, p_map_up);
_query_task_clip_path(p_query_task, apex_poly, right_portal, right_poly);

apex_point = right_portal;
p = right_poly;
Expand All @@ -626,13 +625,13 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue
}
}

if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) {
if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(map_up) <= 0) {
//process
if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) {
if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(map_up) < 0) {
right_poly = p;
right_portal = right;
} else {
clip_path(p_query_task, p_path_corridor, apex_poly, left_portal, left_poly, p_map_up);
_query_task_clip_path(p_query_task, apex_poly, left_portal, left_poly);

apex_point = left_portal;
p = left_poly;
Expand All @@ -655,18 +654,20 @@ void NavMeshQueries3D::_path_corridor_post_process_corridorfunnel(NavMeshPathQue
}

// If the last point is not the begin point, add it to the list.
if (p_query_task.path_points[p_query_task.path_points.size() - 1] != p_begin_point) {
_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_point) {
_query_task_push_back_point_with_metadata(p_query_task, begin_point, p_query_task.begin_poly);
}
}

void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task) {
const Vector3 &begin_point = p_query_task.begin_point;
const Vector3 &end_point = p_query_task.end_point;
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;

_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
_query_task_push_back_point_with_metadata(p_query_task, end_point, p_query_task.end_poly);

// Add mid points.
int np_id = p_least_cost_id;
int np_id = p_query_task.least_cost_id;
while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
if (p_path_corridor[np_id].back_navigation_edge != -1) {
int prev = p_path_corridor[np_id].back_navigation_edge;
Expand All @@ -681,57 +682,67 @@ void NavMeshQueries3D::_path_corridor_post_process_edgecentered(NavMeshPathQuery
np_id = p_path_corridor[np_id].back_navigation_poly_id;
}

_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
_query_task_push_back_point_with_metadata(p_query_task, begin_point, p_query_task.begin_poly);
}

void NavMeshQueries3D::_path_corridor_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task, int p_least_cost_id, const gd::Polygon *p_begin_poly, Vector3 p_begin_point, const gd::Polygon *p_end_polygon, Vector3 p_end_point) {
void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task) {
const Vector3 &begin_point = p_query_task.begin_point;
const Vector3 &end_point = p_query_task.end_point;
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;

_query_task_push_back_point_with_metadata(p_query_task, p_end_point, p_end_polygon);
_query_task_push_back_point_with_metadata(p_query_task, end_point, p_query_task.end_poly);

// Add mid points.
int np_id = p_least_cost_id;
int np_id = p_query_task.least_cost_id;
while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);

np_id = p_path_corridor[np_id].back_navigation_poly_id;
}

_query_task_push_back_point_with_metadata(p_query_task, p_begin_point, p_begin_poly);
_query_task_push_back_point_with_metadata(p_query_task, begin_point, p_query_task.begin_poly);
}

void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons, const gd::Polygon **r_begin_poly, Vector3 &r_begin_point, const gd::Polygon **r_end_poly, Vector3 &r_end_point) {
void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
// Find begin polyon and begin position closest to start position and
// end polyon and end position closest to target position on the map.
real_t begin_d = FLT_MAX;
real_t end_d = FLT_MAX;

Vector3 begin_point;
Vector3 end_point;

// Find the initial poly and the end poly on this map.
for (const gd::Polygon &p : p_polygons) {
for (const gd::Polygon &polygon : p_polygons) {
// Only consider the polygon if it in a region with compatible layers.
if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
if ((p_query_task.navigation_layers & polygon.owner->get_navigation_layers()) == 0) {
continue;
}

// For each face check the distance between the origin/destination.
for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
for (size_t point_id = 2; point_id < polygon.points.size(); point_id++) {
const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);

Vector3 point = face.get_closest_point_to(p_query_task.start_position);
real_t distance_to_point = point.distance_to(p_query_task.start_position);
if (distance_to_point < begin_d) {
begin_d = distance_to_point;
*r_begin_poly = &p;
r_begin_point = point;
p_query_task.begin_poly = &polygon;
begin_point = point;
}

point = face.get_closest_point_to(p_query_task.target_position);
distance_to_point = point.distance_to(p_query_task.target_position);
if (distance_to_point < end_d) {
end_d = distance_to_point;
*r_end_poly = &p;
r_end_point = point;
p_query_task.end_poly = &polygon;
end_point = point;
}
}
}

p_query_task.begin_point = begin_point;
p_query_task.end_point = end_point;
}

Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
Expand Down Expand Up @@ -883,15 +894,17 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Pol
return cp.owner;
}

void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::NavigationPoly> &p_navigation_polys, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, const Vector3 &p_map_up) {
void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) {
const Vector3 &map_up = p_query_task.map_up;
LocalVector<gd::NavigationPoly> &path_corridor = p_query_task.path_query_slot->path_corridor;
Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1];

if (from.is_equal_approx(p_to_point)) {
return;
}

Plane cut_plane;
cut_plane.normal = (from - p_to_point).cross(p_map_up);
cut_plane.normal = (from - p_to_point).cross(map_up);
if (cut_plane.normal == Vector3()) {
return;
}
Expand All @@ -903,7 +916,7 @@ void NavMeshQueries3D::clip_path(NavMeshPathQueryTask3D &p_query_task, const Loc
Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;

ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
from_poly = &path_corridor[from_poly->back_navigation_poly_id];

if (!pathway_start.is_equal_approx(pathway_end)) {
Vector3 inters;
Expand Down
Loading

0 comments on commit dbda4a6

Please sign in to comment.