/**************************************************************************/ /* nav_mesh_queries_2d.cpp */ /**************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /**************************************************************************/ /* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ /* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ /* "Software"), to deal in the Software without restriction, including */ /* without limitation the rights to use, copy, modify, merge, publish, */ /* distribute, sublicense, and/or sell copies of the Software, and to */ /* permit persons to whom the Software is furnished to do so, subject to */ /* the following conditions: */ /* */ /* The above copyright notice and this permission notice shall be */ /* included in all copies or substantial portions of the Software. */ /* */ /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /**************************************************************************/ #include "nav_mesh_queries_2d.h" #include "../nav_base_2d.h" #include "../nav_map_2d.h" #include "../triangle2.h" #include "nav_region_iteration_2d.h" #include "core/math/geometry_2d.h" #include "servers/navigation/navigation_utilities.h" using namespace nav_2d; #define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) bool NavMeshQueries2D::emit_callback(const Callable &p_callback) { ERR_FAIL_COND_V(!p_callback.is_valid(), false); Callable::CallError ce; Variant result; p_callback.callp(nullptr, 0, result, ce); return ce.error == Callable::CallError::CALL_OK; } Vector2 NavMeshQueries2D::polygons_get_random_point(const LocalVector &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) { const LocalVector ®ion_polygons = p_polygons; if (region_polygons.is_empty()) { return Vector2(); } if (p_uniformly) { real_t accumulated_area = 0; RBMap region_area_map; for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) { const Polygon ®ion_polygon = region_polygons[rp_index]; real_t polyon_area = region_polygon.surface_area; if (polyon_area == 0.0) { continue; } region_area_map[accumulated_area] = rp_index; accumulated_area += polyon_area; } if (region_area_map.is_empty() || accumulated_area == 0) { // All polygons have no real surface / no area. return Vector2(); } real_t region_area_map_pos = Math::random(real_t(0), accumulated_area); RBMap::Iterator region_E = region_area_map.find_closest(region_area_map_pos); ERR_FAIL_COND_V(!region_E, Vector2()); uint32_t rrp_polygon_index = region_E->value; ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector2()); const Polygon &rr_polygon = region_polygons[rrp_polygon_index]; real_t accumulated_polygon_area = 0; RBMap polygon_area_map; for (uint32_t rpp_index = 2; rpp_index < rr_polygon.vertices.size(); rpp_index++) { real_t triangle_area = Triangle2(rr_polygon.vertices[0], rr_polygon.vertices[rpp_index - 1], rr_polygon.vertices[rpp_index]).get_area(); if (triangle_area == 0.0) { continue; } polygon_area_map[accumulated_polygon_area] = rpp_index; accumulated_polygon_area += triangle_area; } if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) { // All faces have no real surface / no area. return Vector2(); } real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area); RBMap::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos); ERR_FAIL_COND_V(!polygon_E, Vector2()); uint32_t rrp_face_index = polygon_E->value; ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.vertices.size(), Vector2()); const Triangle2 triangle(rr_polygon.vertices[0], rr_polygon.vertices[rrp_face_index - 1], rr_polygon.vertices[rrp_face_index]); Vector2 triangle_random_position = triangle.get_random_point_inside(); return triangle_random_position; } else { uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1); const Polygon &rr_polygon = region_polygons[rrp_polygon_index]; uint32_t rrp_face_index = Math::random(int(2), rr_polygon.vertices.size() - 1); const Triangle2 triangle(rr_polygon.vertices[0], rr_polygon.vertices[rrp_face_index - 1], rr_polygon.vertices[rrp_face_index]); Vector2 triangle_random_position = triangle.get_random_point_inside(); return triangle_random_position; } } void NavMeshQueries2D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask2D &p_query_task, const Vector2 &p_point, const 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()); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self()); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id()); } p_query_task.path_points.push_back(p_point); } void NavMeshQueries2D::map_query_path(NavMap2D *p_map, const Ref &p_query_parameters, Ref p_query_result, const Callable &p_callback) { ERR_FAIL_NULL(p_map); ERR_FAIL_COND(p_query_parameters.is_null()); ERR_FAIL_COND(p_query_result.is_null()); using namespace NavigationUtilities; NavMeshQueries2D::NavMeshPathQueryTask2D query_task; query_task.start_position = p_query_parameters->get_start_position(); query_task.target_position = p_query_parameters->get_target_position(); query_task.navigation_layers = p_query_parameters->get_navigation_layers(); query_task.callback = p_callback; const TypedArray &_excluded_regions = p_query_parameters->get_excluded_regions(); const TypedArray &_included_regions = p_query_parameters->get_included_regions(); uint32_t _excluded_region_count = _excluded_regions.size(); uint32_t _included_region_count = _included_regions.size(); query_task.exclude_regions = _excluded_region_count > 0; query_task.include_regions = _included_region_count > 0; if (query_task.exclude_regions) { query_task.excluded_regions.resize(_excluded_region_count); for (uint32_t i = 0; i < _excluded_region_count; i++) { query_task.excluded_regions[i] = _excluded_regions[i]; } } if (query_task.include_regions) { query_task.included_regions.resize(_included_region_count); for (uint32_t i = 0; i < _included_region_count; i++) { query_task.included_regions[i] = _included_regions[i]; } } switch (p_query_parameters->get_pathfinding_algorithm()) { case NavigationPathQueryParameters2D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: { query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; } break; default: { WARN_PRINT("No match for used PathfindingAlgorithm - fallback to default"); query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR; } break; } switch (p_query_parameters->get_path_postprocessing()) { case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: { query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; } break; case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: { query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED; } break; case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: { query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_NONE; } break; default: { WARN_PRINT("No match for used PathPostProcessing - fallback to default"); query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL; } break; } query_task.metadata_flags = (int64_t)p_query_parameters->get_metadata_flags(); query_task.simplify_path = p_query_parameters->get_simplify_path(); query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon(); query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_STARTED; p_map->query_path(query_task); p_query_result->set_data( query_task.path_points, query_task.path_meta_point_types, query_task.path_meta_point_rids, query_task.path_meta_point_owners); if (query_task.callback.is_valid()) { if (emit_callback(query_task.callback)) { query_task.status = NavMeshPathQueryTask2D::TaskStatus::CALLBACK_DISPATCHED; } else { query_task.status = NavMeshPathQueryTask2D::TaskStatus::CALLBACK_FAILED; } } } void NavMeshQueries2D::_query_task_find_start_end_positions(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) { real_t begin_d = FLT_MAX; real_t end_d = FLT_MAX; const LocalVector ®ions = p_map_iteration.region_iterations; for (const NavRegionIteration2D ®ion : regions) { if (!region.get_enabled()) { continue; } if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(region.get_self())) { continue; } if (p_query_task.include_regions && !p_query_task.included_regions.has(region.get_self())) { continue; } // Find the initial poly and the end poly on this map. for (const Polygon &p : region.get_navmesh_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) { continue; } // For each triangle check the distance between the origin/destination. for (uint32_t point_id = 2; point_id < p.vertices.size(); point_id++) { const Triangle2 triangle(p.vertices[0], p.vertices[point_id - 1], p.vertices[point_id]); Vector2 point = triangle.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; p_query_task.begin_polygon = &p; p_query_task.begin_position = point; } point = triangle.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; p_query_task.end_polygon = &p; p_query_task.end_position = point; } } } } } void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task) { const Vector2 p_target_position = p_query_task.target_position; const Polygon *begin_poly = p_query_task.begin_polygon; const Polygon *end_poly = p_query_task.end_polygon; Vector2 begin_point = p_query_task.begin_position; Vector2 end_point = p_query_task.end_position; // Heap of polygons to travel next. Heap &traversable_polys = p_query_task.path_query_slot->traversable_polys; traversable_polys.clear(); LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; for (NavigationPoly &polygon : navigation_polys) { polygon.reset(); } // Initialize the matching navigation polygon. NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id]; begin_navigation_poly.poly = begin_poly; begin_navigation_poly.entry = begin_point; begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; begin_navigation_poly.traveled_distance = 0.0; // This is an implementation of the A* algorithm. uint32_t least_cost_id = begin_poly->id; bool found_route = false; const Polygon *reachable_end = nullptr; real_t distance_to_reachable_end = FLT_MAX; bool is_reachable = true; real_t poly_enter_cost = 0.0; while (true) { const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. for (const Edge &edge : least_cost_poly.poly->edges) { // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) { const Edge::Connection &connection = edge.connections[connection_index]; const NavBaseIteration2D *connection_owner = connection.polygon->owner; const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner); if (!owner_is_usable) { continue; } const Vector2 new_entry = Geometry2D::get_closest_point_to_segment(least_cost_poly.entry, connection.pathway_start, connection.pathway_end); 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; // Check if the neighbor polygon has already been processed. NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id]; if (new_traveled_distance < neighbor_poly.traveled_distance) { // Add the polygon to the heap of polygons to traverse next. neighbor_poly.back_navigation_poly_id = least_cost_id; neighbor_poly.back_navigation_edge = connection.edge; neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start; neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end; neighbor_poly.traveled_distance = new_traveled_distance; neighbor_poly.distance_to_destination = new_entry.distance_to(end_point) * connection_owner->get_travel_cost(); neighbor_poly.entry = new_entry; if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) { traversable_polys.shift(neighbor_poly.traversable_poly_index); } else { neighbor_poly.poly = connection.polygon; traversable_polys.push(&neighbor_poly); } } } } poly_enter_cost = 0; // When the heap of traversable polygons is empty at this point it means the end polygon is // unreachable. if (traversable_polys.is_empty()) { // Thus use the further reachable polygon ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); is_reachable = false; if (reachable_end == nullptr) { // The path is not found and there is not a way out. break; } // Set as end point the furthest reachable point. end_poly = reachable_end; real_t end_d = FLT_MAX; for (uint32_t point_id = 2; point_id < end_poly->vertices.size(); point_id++) { Triangle2 t(end_poly->vertices[0], end_poly->vertices[point_id - 1], end_poly->vertices[point_id]); Vector2 spoint = t.get_closest_point_to(p_target_position); real_t dpoint = spoint.distance_squared_to(p_target_position); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; } } // Search all faces of start polygon as well. bool closest_point_on_start_poly = false; for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) { Triangle2 t(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]); Vector2 spoint = t.get_closest_point_to(p_target_position); real_t dpoint = spoint.distance_squared_to(p_target_position); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; closest_point_on_start_poly = true; } } if (closest_point_on_start_poly) { // No point to run PostProcessing when start and end convex polygon is the same. p_query_task.path_clear(); _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly); _query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly); p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; return; } for (NavigationPoly &nav_poly : navigation_polys) { nav_poly.poly = nullptr; nav_poly.traveled_distance = FLT_MAX; } navigation_polys[begin_poly->id].poly = begin_poly; navigation_polys[begin_poly->id].traveled_distance = 0; least_cost_id = begin_poly->id; reachable_end = nullptr; } else { // Pop the polygon with the lowest travel cost from the heap of traversable polygons. least_cost_id = traversable_polys.pop()->poly->id; // Store the farthest reachable end polygon in case our goal is not reachable. if (is_reachable) { real_t distance = navigation_polys[least_cost_id].entry.distance_squared_to(p_target_position); if (distance_to_reachable_end > distance) { distance_to_reachable_end = distance; reachable_end = navigation_polys[least_cost_id].poly; } } // Check if we reached the end if (navigation_polys[least_cost_id].poly == end_poly) { found_route = true; break; } if (navigation_polys[least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) { poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); } } } // We did not find a route but we have both a start polygon and an end polygon at this point. // Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon. if (!found_route) { real_t end_d = FLT_MAX; // Search all faces of the start polygon for the closest point to our target position. for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) { Triangle2 t(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]); Vector2 spoint = t.get_closest_point_to(p_target_position); real_t dpoint = spoint.distance_squared_to(p_target_position); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; } } p_query_task.path_clear(); _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly); _query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly); p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; } else { p_query_task.end_position = end_point; p_query_task.end_polygon = end_poly; p_query_task.begin_position = begin_point; p_query_task.begin_polygon = begin_poly; p_query_task.least_cost_id = least_cost_id; } } void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) { p_query_task.path_clear(); _query_task_find_start_end_positions(p_query_task, p_map_iteration); // Check for trivial cases. if (!p_query_task.begin_polygon || !p_query_task.end_polygon) { p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; return; } if (p_query_task.begin_polygon == p_query_task.end_polygon) { p_query_task.path_clear(); _query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon); _query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon); p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; return; } _query_task_build_path_corridor(p_query_task); if (p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FAILED) { return; } // Post-Process path. switch (p_query_task.path_postprocessing) { case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: { _query_task_post_process_corridorfunnel(p_query_task); } break; case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: { _query_task_post_process_edgecentered(p_query_task); } break; case PathPostProcessing::PATH_POSTPROCESSING_NONE: { _query_task_post_process_nopostprocessing(p_query_task); } break; default: { WARN_PRINT("No match for used PathPostProcessing - fallback to default"); _query_task_post_process_corridorfunnel(p_query_task); } break; } p_query_task.path_reverse(); if (p_query_task.simplify_path) { _query_task_simplified_path_points(p_query_task); } #ifdef DEBUG_ENABLED // Ensure post conditions as path meta arrays if used MUST match in array size with the path points. if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size()); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size()); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size()); } #endif // DEBUG_ENABLED p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED; } void NavMeshQueries2D::_query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task) { if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) { return; } const LocalVector &simplified_path_indices = NavMeshQueries2D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon); uint32_t index_count = simplified_path_indices.size(); { Vector2 *points_ptr = p_query_task.path_points.ptr(); for (uint32_t i = 0; i < index_count; i++) { points_ptr[i] = points_ptr[simplified_path_indices[i]]; } p_query_task.path_points.resize(index_count); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { int32_t *types_ptr = p_query_task.path_meta_point_types.ptr(); for (uint32_t i = 0; i < index_count; i++) { types_ptr[i] = types_ptr[simplified_path_indices[i]]; } p_query_task.path_meta_point_types.resize(index_count); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { RID *rids_ptr = p_query_task.path_meta_point_rids.ptr(); for (uint32_t i = 0; i < index_count; i++) { rids_ptr[i] = rids_ptr[simplified_path_indices[i]]; } p_query_task.path_meta_point_rids.resize(index_count); } if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { int64_t *owners_ptr = p_query_task.path_meta_point_owners.ptr(); for (uint32_t i = 0; i < index_count; i++) { owners_ptr[i] = owners_ptr[simplified_path_indices[i]]; } p_query_task.path_meta_point_owners.resize(index_count); } } void NavMeshQueries2D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask2D &p_query_task) { Vector2 end_point = p_query_task.end_position; const Polygon *end_poly = p_query_task.end_polygon; Vector2 begin_point = p_query_task.begin_position; const Polygon *begin_poly = p_query_task.begin_polygon; uint32_t least_cost_id = p_query_task.least_cost_id; LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; // Set the apex poly/point to the end point. NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; const Vector2 back_edge_closest_point = Geometry2D::get_closest_point_to_segment(end_point, apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end); 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 = &navigation_polys[apex_poly->back_navigation_poly_id]; } } Vector2 apex_point = end_point; NavigationPoly *left_poly = apex_poly; Vector2 left_portal = apex_point; NavigationPoly *right_poly = apex_poly; Vector2 right_portal = apex_point; NavigationPoly *p = apex_poly; _query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly); while (p) { // Set left and right points of the pathway between polygons. Vector2 left = p->back_navigation_edge_pathway_start; Vector2 right = p->back_navigation_edge_pathway_end; if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right) < 0) { SWAP(left, right); } bool skip = false; if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left) >= 0) { // Process. if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal) > 0) { left_poly = p; left_portal = left; } else { _query_task_clip_path(p_query_task, apex_poly, right_portal, right_poly); apex_point = right_portal; p = right_poly; left_poly = p; apex_poly = p; left_portal = apex_point; right_portal = apex_point; _query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly); skip = true; } } if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right) <= 0) { // Process. if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal) < 0) { right_poly = p; right_portal = right; } else { _query_task_clip_path(p_query_task, apex_poly, left_portal, left_poly); apex_point = left_portal; p = left_poly; right_poly = p; apex_poly = p; right_portal = apex_point; left_portal = apex_point; _query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly); } } // Go to the previous polygon. if (p->back_navigation_poly_id != -1) { p = &navigation_polys[p->back_navigation_poly_id]; } else { // The end p = nullptr; } } // 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] != begin_point) { _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly); } } void NavMeshQueries2D::_query_task_post_process_edgecentered(NavMeshPathQueryTask2D &p_query_task) { Vector2 end_point = p_query_task.end_position; const Polygon *end_poly = p_query_task.end_polygon; Vector2 begin_point = p_query_task.begin_position; const Polygon *begin_poly = p_query_task.begin_polygon; uint32_t least_cost_id = p_query_task.least_cost_id; LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; _query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly); // Add mid points. int np_id = least_cost_id; while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { if (navigation_polys[np_id].back_navigation_edge != -1) { int prev = navigation_polys[np_id].back_navigation_edge; int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->vertices.size(); Vector2 point = (navigation_polys[np_id].poly->vertices[prev] + navigation_polys[np_id].poly->vertices[prev_n]) * 0.5; _query_task_push_back_point_with_metadata(p_query_task, point, navigation_polys[np_id].poly); } else { _query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly); } np_id = navigation_polys[np_id].back_navigation_poly_id; } _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly); } void NavMeshQueries2D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask2D &p_query_task) { Vector2 end_point = p_query_task.end_position; const Polygon *end_poly = p_query_task.end_polygon; Vector2 begin_point = p_query_task.begin_position; const Polygon *begin_poly = p_query_task.begin_polygon; uint32_t least_cost_id = p_query_task.least_cost_id; LocalVector &navigation_polys = p_query_task.path_query_slot->path_corridor; _query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly); // Add mid points. int np_id = least_cost_id; while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { _query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly); np_id = navigation_polys[np_id].back_navigation_poly_id; } _query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly); } Vector2 NavMeshQueries2D::map_iteration_get_closest_point(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) { ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point); return cp.point; } RID NavMeshQueries2D::map_iteration_get_closest_point_owner(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) { ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point); return cp.owner; } ClosestPointQueryResult NavMeshQueries2D::map_iteration_get_closest_point_info(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) { ClosestPointQueryResult result; real_t closest_point_distance_squared = FLT_MAX; // TODO: Check for further 2D improvements. const LocalVector ®ions = p_map_iteration.region_iterations; for (const NavRegionIteration2D ®ion : regions) { for (const Polygon &polygon : region.get_navmesh_polygons()) { real_t cross = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); Vector2 closest_on_polygon; real_t closest = FLT_MAX; bool inside = true; Vector2 previous = polygon.vertices[polygon.vertices.size() - 1]; for (uint32_t point_id = 0; point_id < polygon.vertices.size(); ++point_id) { Vector2 edge = polygon.vertices[point_id] - previous; Vector2 to_point = p_point - previous; real_t edge_to_point_cross = edge.cross(to_point); bool clockwise = (edge_to_point_cross * cross) > 0; // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge. if (!clockwise) { inside = false; real_t point_projected_on_edge = edge.dot(to_point); real_t edge_square = edge.length_squared(); if (point_projected_on_edge > edge_square) { real_t distance = polygon.vertices[point_id].distance_squared_to(p_point); if (distance < closest) { closest_on_polygon = polygon.vertices[point_id]; closest = distance; } } else if (point_projected_on_edge < 0.0) { real_t distance = previous.distance_squared_to(p_point); if (distance < closest) { closest_on_polygon = previous; closest = distance; } } else { // If we project on this edge, this will be the closest point. real_t percent = point_projected_on_edge / edge_square; closest_on_polygon = previous + percent * edge; break; } } previous = polygon.vertices[point_id]; } if (inside) { closest_point_distance_squared = 0.0; result.point = p_point; result.owner = polygon.owner->get_self(); break; } else { real_t distance = closest_on_polygon.distance_squared_to(p_point); if (distance < closest_point_distance_squared) { closest_point_distance_squared = distance; result.point = closest_on_polygon; result.owner = polygon.owner->get_self(); } } } } return result; } Vector2 NavMeshQueries2D::map_iteration_get_random_point(const NavMapIteration2D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) { if (p_map_iteration.region_iterations.is_empty()) { return Vector2(); } LocalVector accessible_regions; accessible_regions.reserve(p_map_iteration.region_iterations.size()); for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) { const NavRegionIteration2D ®ion = p_map_iteration.region_iterations[i]; if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) { continue; } accessible_regions.push_back(i); } if (accessible_regions.is_empty()) { // All existing region polygons are disabled. return Vector2(); } if (p_uniformly) { real_t accumulated_region_surface_area = 0; RBMap accessible_regions_area_map; for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) { const NavRegionIteration2D ®ion = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]]; real_t region_surface_area = region.surface_area; if (region_surface_area == 0.0f) { continue; } accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index; accumulated_region_surface_area += region_surface_area; } if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) { // All faces have no real surface / no area. return Vector2(); } real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area); RBMap::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map); ERR_FAIL_COND_V(!E, Vector2()); uint32_t random_region_index = E->value; ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector2()); const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]]; return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly); } else { uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1); const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]]; return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly); } } Vector2 NavMeshQueries2D::polygons_get_closest_point(const LocalVector &p_polygons, const Vector2 &p_point) { ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); return cp.point; } ClosestPointQueryResult NavMeshQueries2D::polygons_get_closest_point_info(const LocalVector &p_polygons, const Vector2 &p_point) { ClosestPointQueryResult result; real_t closest_point_distance_squared = FLT_MAX; // TODO: Check for further 2D improvements. for (const Polygon &polygon : p_polygons) { real_t cross = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); Vector2 closest_on_polygon; real_t closest = FLT_MAX; bool inside = true; Vector2 previous = polygon.vertices[polygon.vertices.size() - 1]; for (uint32_t point_id = 0; point_id < polygon.vertices.size(); ++point_id) { Vector2 edge = polygon.vertices[point_id] - previous; Vector2 to_point = p_point - previous; real_t edge_to_point_cross = edge.cross(to_point); bool clockwise = (edge_to_point_cross * cross) > 0; // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge. if (!clockwise) { inside = false; real_t point_projected_on_edge = edge.dot(to_point); real_t edge_square = edge.length_squared(); if (point_projected_on_edge > edge_square) { real_t distance = polygon.vertices[point_id].distance_squared_to(p_point); if (distance < closest) { closest_on_polygon = polygon.vertices[point_id]; closest = distance; } } else if (point_projected_on_edge < 0.0) { real_t distance = previous.distance_squared_to(p_point); if (distance < closest) { closest_on_polygon = previous; closest = distance; } } else { // If we project on this edge, this will be the closest point. real_t percent = point_projected_on_edge / edge_square; closest_on_polygon = previous + percent * edge; break; } } previous = polygon.vertices[point_id]; } if (inside) { closest_point_distance_squared = 0.0; result.point = p_point; result.owner = polygon.owner->get_self(); break; } else { real_t distance = closest_on_polygon.distance_squared_to(p_point); if (distance < closest_point_distance_squared) { closest_point_distance_squared = distance; result.point = closest_on_polygon; result.owner = polygon.owner->get_self(); } } } return result; } RID NavMeshQueries2D::polygons_get_closest_point_owner(const LocalVector &p_polygons, const Vector2 &p_point) { ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); return cp.owner; } static bool _line_intersects_segment(const Vector2 &p_line_normal, real_t p_line_d, const Vector2 &p_segment_begin, const Vector2 &p_segment_end, Vector2 &r_intersection) { Vector2 segment = p_segment_begin - p_segment_end; real_t den = p_line_normal.dot(segment); if (Math::is_zero_approx(den)) { return false; } real_t dist = (p_line_normal.dot(p_segment_begin) - p_line_d) / den; if (dist < (real_t)-CMP_EPSILON || dist > (1.0 + (real_t)CMP_EPSILON)) { return false; } r_intersection = p_segment_begin - segment * dist; return true; } void NavMeshQueries2D::_query_task_clip_path(NavMeshPathQueryTask2D &p_query_task, const NavigationPoly *p_from_poly, const Vector2 &p_to_point, const NavigationPoly *p_to_poly) { Vector2 from = p_query_task.path_points[p_query_task.path_points.size() - 1]; const LocalVector &p_navigation_polys = p_query_task.path_query_slot->path_corridor; if (from.is_equal_approx(p_to_point)) { return; } // Compute line parameters (equivalent to the Plane case in 3D). const Vector2 normal = -(from - p_to_point).orthogonal().normalized(); const real_t d = normal.dot(from); while (p_from_poly != p_to_poly) { Vector2 pathway_start = p_from_poly->back_navigation_edge_pathway_start; Vector2 pathway_end = p_from_poly->back_navigation_edge_pathway_end; ERR_FAIL_COND(p_from_poly->back_navigation_poly_id == -1); p_from_poly = &p_navigation_polys[p_from_poly->back_navigation_poly_id]; if (!pathway_start.is_equal_approx(pathway_end)) { Vector2 inters; if (_line_intersects_segment(normal, d, pathway_start, pathway_end, inters)) { if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(p_query_task.path_points[p_query_task.path_points.size() - 1])) { _query_task_push_back_point_with_metadata(p_query_task, inters, p_from_poly->poly); } } } } } bool NavMeshQueries2D::_query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner) { bool owner_usable = true; if ((p_query_task.navigation_layers & p_owner->get_navigation_layers()) == 0) { // Not usable. No matching bit between task filter bitmask and owner bitmask. owner_usable = false; return owner_usable; } if (p_query_task.exclude_regions || p_query_task.include_regions) { switch (p_owner->get_type()) { case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION: { if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(p_owner->get_self())) { // Not usable. Exclude region filter is active and this region is excluded. owner_usable = false; } else if (p_query_task.include_regions && !p_query_task.included_regions.has(p_owner->get_self())) { // Not usable. Include region filter is active and this region is not included. owner_usable = false; } } break; case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: { const LocalVector &link_polygons = p_owner->get_navmesh_polygons(); if (link_polygons.size() != 2) { // Not usable. Whatever this is, it is not a valid connected link. owner_usable = false; } else { const RID link_start_region = link_polygons[0].owner->get_self(); const RID link_end_region = link_polygons[1].owner->get_self(); if (p_query_task.exclude_regions && (p_query_task.excluded_regions.has(link_start_region) || p_query_task.excluded_regions.has(link_end_region))) { // Not usable. Exclude region filter is active and at least one region of the link is excluded. owner_usable = false; } if (p_query_task.include_regions && (!p_query_task.included_regions.has(link_start_region) || !p_query_task.excluded_regions.has(link_end_region))) { // Not usable. Include region filter is active and not both regions of the links are included. owner_usable = false; } } } break; } } return owner_usable; } LocalVector NavMeshQueries2D::get_simplified_path_indices(const LocalVector &p_path, real_t p_epsilon) { p_epsilon = MAX(0.0, p_epsilon); real_t squared_epsilon = p_epsilon * p_epsilon; LocalVector simplified_path_indices; simplified_path_indices.reserve(p_path.size()); simplified_path_indices.push_back(0); simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, simplified_path_indices); simplified_path_indices.push_back(p_path.size() - 1); return simplified_path_indices; } void NavMeshQueries2D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector &p_points, real_t p_epsilon, LocalVector &r_simplified_path_indices) { real_t point_max_distance = 0.0; int point_max_index = 0; for (int i = p_start_inx; i < p_end_inx; i++) { const Vector2 &checked_point = p_points[i]; const Vector2 closest_point = Geometry2D::get_closest_point_to_segment(checked_point, p_points[p_start_inx], p_points[p_end_inx]); real_t distance_squared = closest_point.distance_squared_to(checked_point); if (distance_squared > point_max_distance) { point_max_index = i; point_max_distance = distance_squared; } } if (point_max_distance > p_epsilon) { simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_simplified_path_indices); r_simplified_path_indices.push_back(point_max_index); simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_simplified_path_indices); } }