From a4cfc77dc070d93840c186509d795b17b635bf93 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Fri, 30 Aug 2024 15:52:29 +0200 Subject: [PATCH] Move NavigationServer mesh queries to dedicated file Moves all the navigation mesh query related functions from NavMap and NavRegion to a dedicated file and makes them static. --- modules/navigation/3d/nav_mesh_queries_3d.cpp | 715 ++++++++++++++++++ modules/navigation/3d/nav_mesh_queries_3d.h | 54 ++ modules/navigation/nav_map.cpp | 594 +-------------- modules/navigation/nav_map.h | 1 - modules/navigation/nav_region.cpp | 78 +- 5 files changed, 785 insertions(+), 657 deletions(-) create mode 100644 modules/navigation/3d/nav_mesh_queries_3d.cpp create mode 100644 modules/navigation/3d/nav_mesh_queries_3d.h diff --git a/modules/navigation/3d/nav_mesh_queries_3d.cpp b/modules/navigation/3d/nav_mesh_queries_3d.cpp new file mode 100644 index 00000000000..70207f86ced --- /dev/null +++ b/modules/navigation/3d/nav_mesh_queries_3d.cpp @@ -0,0 +1,715 @@ +/**************************************************************************/ +/* nav_mesh_queries_3d.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. */ +/**************************************************************************/ + +#ifndef _3D_DISABLED + +#include "nav_mesh_queries_3d.h" + +#include "../nav_base.h" + +#include "core/math/geometry_3d.h" + +#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) + +#define APPEND_METADATA(poly) \ + if (r_path_types) { \ + r_path_types->push_back(poly->owner->get_type()); \ + } \ + if (r_path_rids) { \ + r_path_rids->push_back(poly->owner->get_self()); \ + } \ + if (r_path_owners) { \ + r_path_owners->push_back(poly->owner->get_owner_id()); \ + } + +Vector3 NavMeshQueries3D::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 Vector3(); + } + + 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 gd::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 Vector3(); + } + + 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, Vector3()); + uint32_t rrp_polygon_index = region_E->value; + ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector3()); + + const gd::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.points.size(); rpp_index++) { + real_t face_area = Face3(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area(); + + if (face_area == 0.0) { + continue; + } + polygon_area_map[accumulated_polygon_area] = rpp_index; + accumulated_polygon_area += face_area; + } + if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) { + // All faces have no real surface / no area. + return Vector3(); + } + + 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, Vector3()); + uint32_t rrp_face_index = polygon_E->value; + ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector3()); + + const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); + + Vector3 face_random_position = face.get_random_point_inside(); + return face_random_position; + + } else { + uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1); + + const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index]; + + uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1); + + const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); + + Vector3 face_random_position = face.get_random_point_inside(); + return face_random_position; + } +} + +Vector NavMeshQueries3D::polygons_get_path(const LocalVector &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size) { + // Clear metadata outputs. + if (r_path_types) { + r_path_types->clear(); + } + if (r_path_rids) { + r_path_rids->clear(); + } + if (r_path_owners) { + r_path_owners->clear(); + } + + // Find the start poly and the end poly on this map. + const gd::Polygon *begin_poly = nullptr; + const gd::Polygon *end_poly = nullptr; + Vector3 begin_point; + Vector3 end_point; + real_t begin_d = FLT_MAX; + real_t end_d = FLT_MAX; + // Find the initial poly and the end poly on this map. + for (const gd::Polygon &p : p_polygons) { + // Only consider the polygon if it in a region with compatible layers. + if ((p_navigation_layers & p.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); + + Vector3 point = face.get_closest_point_to(p_origin); + real_t distance_to_point = point.distance_to(p_origin); + if (distance_to_point < begin_d) { + begin_d = distance_to_point; + begin_poly = &p; + begin_point = point; + } + + point = face.get_closest_point_to(p_destination); + distance_to_point = point.distance_to(p_destination); + if (distance_to_point < end_d) { + end_d = distance_to_point; + end_poly = &p; + end_point = point; + } + } + } + + // Check for trivial cases + if (!begin_poly || !end_poly) { + return Vector(); + } + if (begin_poly == end_poly) { + if (r_path_types) { + r_path_types->resize(2); + r_path_types->write[0] = begin_poly->owner->get_type(); + r_path_types->write[1] = end_poly->owner->get_type(); + } + + if (r_path_rids) { + r_path_rids->resize(2); + (*r_path_rids)[0] = begin_poly->owner->get_self(); + (*r_path_rids)[1] = end_poly->owner->get_self(); + } + + if (r_path_owners) { + r_path_owners->resize(2); + r_path_owners->write[0] = begin_poly->owner->get_owner_id(); + r_path_owners->write[1] = end_poly->owner->get_owner_id(); + } + + Vector path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + // List of all reachable navigation polys. + LocalVector navigation_polys; + navigation_polys.resize(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; + 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; + + // Heap of polygons to travel next. + gd::Heap + traversable_polys; + traversable_polys.reserve(p_polygons.size() * 0.25); + + // This is an implementation of the A* algorithm. + int least_cost_id = begin_poly->id; + int prev_least_cost_id = -1; + bool found_route = false; + + const gd::Polygon *reachable_end = nullptr; + real_t distance_to_reachable_end = FLT_MAX; + bool is_reachable = true; + + while (true) { + // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. + for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) { + // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. + for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { + const gd::Edge::Connection &connection = edge.connections[connection_index]; + + // Only consider the connection to another polygon if this polygon is in a region with compatible layers. + if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { + continue; + } + + const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; + real_t poly_enter_cost = 0.0; + real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); + + if (prev_least_cost_id != -1 && navigation_polys[prev_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(); + } + prev_least_cost_id = least_cost_id; + + 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 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. + gd::NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id]; + if (neighbor_poly.poly != nullptr) { + // If the neighbor polygon hasn't been traversed yet and the new path leading to + // it is shorter, update the polygon. + if (neighbor_poly.traversable_poly_index < traversable_polys.size() && + new_traveled_distance < neighbor_poly.traveled_distance) { + 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) * + neighbor_poly.poly->owner->get_travel_cost(); + neighbor_poly.entry = new_entry; + + // Update the priority of the polygon in the heap. + traversable_polys.shift(neighbor_poly.traversable_poly_index); + } + } else { + // Initialize the matching navigation polygon. + neighbor_poly.poly = connection.polygon; + 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) * + neighbor_poly.poly->owner->get_travel_cost(); + neighbor_poly.entry = new_entry; + + // Add the polygon to the heap of polygons to traverse next. + traversable_polys.push(&neighbor_poly); + } + } + } + + // 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; + end_d = FLT_MAX; + for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { + Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_destination); + real_t dpoint = spoint.distance_to(p_destination); + 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 (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { + Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_destination); + real_t dpoint = spoint.distance_to(p_destination); + 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. + if (r_path_types) { + r_path_types->resize(2); + r_path_types->write[0] = begin_poly->owner->get_type(); + r_path_types->write[1] = begin_poly->owner->get_type(); + } + + if (r_path_rids) { + r_path_rids->resize(2); + (*r_path_rids)[0] = begin_poly->owner->get_self(); + (*r_path_rids)[1] = begin_poly->owner->get_self(); + } + + if (r_path_owners) { + r_path_owners->resize(2); + r_path_owners->write[0] = begin_poly->owner->get_owner_id(); + r_path_owners->write[1] = begin_poly->owner->get_owner_id(); + } + + Vector path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + for (gd::NavigationPoly &nav_poly : navigation_polys) { + nav_poly.poly = nullptr; + } + navigation_polys[begin_poly->id].poly = begin_poly; + + least_cost_id = begin_poly->id; + prev_least_cost_id = -1; + + reachable_end = nullptr; + + continue; + } + + // 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_to(p_destination); + 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; + } + } + + // 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) { + end_d = FLT_MAX; + // Search all faces of the start polygon for the closest point to our target position. + for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { + Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_destination); + real_t dpoint = spoint.distance_to(p_destination); + if (dpoint < end_d) { + end_point = spoint; + end_d = dpoint; + } + } + + if (r_path_types) { + r_path_types->resize(2); + r_path_types->write[0] = begin_poly->owner->get_type(); + r_path_types->write[1] = begin_poly->owner->get_type(); + } + + if (r_path_rids) { + r_path_rids->resize(2); + (*r_path_rids)[0] = begin_poly->owner->get_self(); + (*r_path_rids)[1] = begin_poly->owner->get_self(); + } + + if (r_path_owners) { + r_path_owners->resize(2); + r_path_owners->write[0] = begin_poly->owner->get_owner_id(); + r_path_owners->write[1] = begin_poly->owner->get_owner_id(); + } + + Vector path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + Vector path; + // Optimize the path. + if (p_optimize) { + // Set the apex poly/point to the end point + gd::NavigationPoly *apex_poly = &navigation_polys[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(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 = &navigation_polys[apex_poly->back_navigation_poly_id]; + } + } + + Vector3 apex_point = end_point; + + gd::NavigationPoly *left_poly = apex_poly; + Vector3 left_portal = apex_point; + gd::NavigationPoly *right_poly = apex_poly; + Vector3 right_portal = apex_point; + + gd::NavigationPoly *p = apex_poly; + + path.push_back(end_point); + APPEND_METADATA(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) { + SWAP(left, right); + } + + bool skip = false; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(p_map_up) >= 0) { + //process + if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(p_map_up) > 0) { + left_poly = p; + left_portal = left; + } else { + clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners, p_map_up); + + apex_point = right_portal; + p = right_poly; + left_poly = p; + apex_poly = p; + left_portal = apex_point; + right_portal = apex_point; + + path.push_back(apex_point); + APPEND_METADATA(apex_poly->poly); + skip = true; + } + } + + if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(p_map_up) <= 0) { + //process + if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(p_map_up) < 0) { + right_poly = p; + right_portal = right; + } else { + clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners, p_map_up); + + apex_point = left_portal; + p = left_poly; + right_poly = p; + apex_poly = p; + right_portal = apex_point; + left_portal = apex_point; + + path.push_back(apex_point); + APPEND_METADATA(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 (path[path.size() - 1] != begin_point) { + path.push_back(begin_point); + APPEND_METADATA(begin_poly); + } + + path.reverse(); + if (r_path_types) { + r_path_types->reverse(); + } + if (r_path_rids) { + r_path_rids->reverse(); + } + if (r_path_owners) { + r_path_owners->reverse(); + } + + } else { + path.push_back(end_point); + APPEND_METADATA(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->points.size(); + Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; + + path.push_back(point); + APPEND_METADATA(navigation_polys[np_id].poly); + } else { + path.push_back(navigation_polys[np_id].entry); + APPEND_METADATA(navigation_polys[np_id].poly); + } + + np_id = navigation_polys[np_id].back_navigation_poly_id; + } + + path.push_back(begin_point); + APPEND_METADATA(begin_poly); + + path.reverse(); + if (r_path_types) { + r_path_types->reverse(); + } + if (r_path_rids) { + r_path_rids->reverse(); + } + if (r_path_owners) { + r_path_owners->reverse(); + } + } + + // Ensure post conditions (path arrays MUST match in size). + CRASH_COND(r_path_types && path.size() != r_path_types->size()); + CRASH_COND(r_path_rids && path.size() != r_path_rids->size()); + CRASH_COND(r_path_owners && path.size() != r_path_owners->size()); + + return path; +} + +Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) { + bool use_collision = p_use_collision; + Vector3 closest_point; + real_t closest_point_distance = FLT_MAX; + + for (const gd::Polygon &polygon : p_polygons) { + // For each face check the distance to the segment. + for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) { + const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos); + Vector3 intersection_point; + if (face.intersects_segment(p_from, p_to, &intersection_point)) { + const real_t d = p_from.distance_to(intersection_point); + if (!use_collision) { + closest_point = intersection_point; + use_collision = true; + closest_point_distance = d; + } else if (closest_point_distance > d) { + closest_point = intersection_point; + closest_point_distance = d; + } + } + // If segment does not itersect face, check the distance from segment's endpoints. + else if (!use_collision) { + const Vector3 p_from_closest = face.get_closest_point_to(p_from); + const real_t d_p_from = p_from.distance_to(p_from_closest); + if (closest_point_distance > d_p_from) { + closest_point = p_from_closest; + closest_point_distance = d_p_from; + } + + const Vector3 p_to_closest = face.get_closest_point_to(p_to); + const real_t d_p_to = p_to.distance_to(p_to_closest); + if (closest_point_distance > d_p_to) { + closest_point = p_to_closest; + closest_point_distance = d_p_to; + } + } + } + // Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment. + if (!use_collision) { + for (size_t point_id = 0; point_id < polygon.points.size(); point_id += 1) { + Vector3 a, b; + + Geometry3D::get_closest_points_between_segments( + p_from, + p_to, + polygon.points[point_id].pos, + polygon.points[(point_id + 1) % polygon.points.size()].pos, + a, + b); + + const real_t d = a.distance_to(b); + if (d < closest_point_distance) { + closest_point_distance = d; + closest_point = b; + } + } + } + } + + return closest_point; +} + +Vector3 NavMeshQueries3D::polygons_get_closest_point(const LocalVector &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); + return cp.point; +} + +Vector3 NavMeshQueries3D::polygons_get_closest_point_normal(const LocalVector &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); + return cp.normal; +} + +gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const LocalVector &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult result; + real_t closest_point_distance_squared = FLT_MAX; + + for (const gd::Polygon &polygon : p_polygons) { + for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) { + const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos); + const Vector3 closest_point_on_face = face.get_closest_point_to(p_point); + const real_t distance_squared_to_point = closest_point_on_face.distance_squared_to(p_point); + if (distance_squared_to_point < closest_point_distance_squared) { + result.point = closest_point_on_face; + result.normal = face.get_plane().normal; + result.owner = polygon.owner->get_self(); + closest_point_distance_squared = distance_squared_to_point; + } + } + } + + return result; +} + +RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector &p_polygons, const Vector3 &p_point) { + gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point); + return cp.owner; +} + +void NavMeshQueries3D::clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up) { + Vector3 from = path[path.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); + if (cut_plane.normal == Vector3()) { + return; + } + cut_plane.normal.normalize(); + cut_plane.d = cut_plane.normal.dot(from); + + while (from_poly != p_to_poly) { + Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start; + 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]; + + if (!pathway_start.is_equal_approx(pathway_end)) { + Vector3 inters; + if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { + if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) { + path.push_back(inters); + APPEND_METADATA(from_poly->poly); + } + } + } + } +} + +#endif // _3D_DISABLED diff --git a/modules/navigation/3d/nav_mesh_queries_3d.h b/modules/navigation/3d/nav_mesh_queries_3d.h new file mode 100644 index 00000000000..109bb2f9712 --- /dev/null +++ b/modules/navigation/3d/nav_mesh_queries_3d.h @@ -0,0 +1,54 @@ +/**************************************************************************/ +/* nav_mesh_queries_3d.h */ +/**************************************************************************/ +/* 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. */ +/**************************************************************************/ + +#ifndef NAV_MESH_QUERIES_3D_H +#define NAV_MESH_QUERIES_3D_H + +#ifndef _3D_DISABLED + +#include "../nav_map.h" + +class NavMeshQueries3D { +public: + static Vector3 polygons_get_random_point(const LocalVector &p_polygons, uint32_t p_navigation_layers, bool p_uniformly); + + static Vector polygons_get_path(const LocalVector &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size); + static Vector3 polygons_get_closest_point_to_segment(const LocalVector &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision); + static Vector3 polygons_get_closest_point(const LocalVector &p_polygons, const Vector3 &p_point); + static Vector3 polygons_get_closest_point_normal(const LocalVector &p_polygons, const Vector3 &p_point); + static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector &p_polygons, const Vector3 &p_point); + static RID polygons_get_closest_point_owner(const LocalVector &p_polygons, const Vector3 &p_point); + + static void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners, const Vector3 &p_map_up); +}; + +#endif // _3D_DISABLED + +#endif // NAV_MESH_QUERIES_3D_H diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index f89f5b5812e..dd77e81b452 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -35,25 +35,13 @@ #include "nav_obstacle.h" #include "nav_region.h" +#include "3d/nav_mesh_queries_3d.h" + #include "core/config/project_settings.h" #include "core/object/worker_thread_pool.h" #include -#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) - -// Helper macro -#define APPEND_METADATA(poly) \ - if (r_path_types) { \ - r_path_types->push_back(poly->owner->get_type()); \ - } \ - if (r_path_rids) { \ - r_path_rids->push_back(poly->owner->get_self()); \ - } \ - if (r_path_owners) { \ - r_path_owners->push_back(poly->owner->get_owner_id()); \ - } - #ifdef DEBUG_ENABLED #define NAVMAP_ITERATION_ZERO_ERROR_MSG() \ ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\ @@ -142,455 +130,9 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p return Vector(); } - // Clear metadata outputs. - if (r_path_types) { - r_path_types->clear(); - } - if (r_path_rids) { - r_path_rids->clear(); - } - if (r_path_owners) { - r_path_owners->clear(); - } - - // Find the start poly and the end poly on this map. - const gd::Polygon *begin_poly = nullptr; - const gd::Polygon *end_poly = nullptr; - Vector3 begin_point; - Vector3 end_point; - real_t begin_d = FLT_MAX; - real_t end_d = FLT_MAX; - // Find the initial poly and the end poly on this map. - for (const gd::Polygon &p : polygons) { - // Only consider the polygon if it in a region with compatible layers. - if ((p_navigation_layers & p.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); - - Vector3 point = face.get_closest_point_to(p_origin); - real_t distance_to_point = point.distance_to(p_origin); - if (distance_to_point < begin_d) { - begin_d = distance_to_point; - begin_poly = &p; - begin_point = point; - } - - point = face.get_closest_point_to(p_destination); - distance_to_point = point.distance_to(p_destination); - if (distance_to_point < end_d) { - end_d = distance_to_point; - end_poly = &p; - end_point = point; - } - } - } - - // Check for trivial cases - if (!begin_poly || !end_poly) { - return Vector(); - } - if (begin_poly == end_poly) { - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = end_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = end_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = end_poly->owner->get_owner_id(); - } - - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - // List of all reachable navigation polys. - LocalVector navigation_polys; - navigation_polys.resize(polygons.size() + link_polygons.size()); - - // Initialize the matching navigation polygon. - gd::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; - - // Heap of polygons to travel next. - gd::Heap - traversable_polys; - traversable_polys.reserve(polygons.size() * 0.25); - - // This is an implementation of the A* algorithm. - int least_cost_id = begin_poly->id; - int prev_least_cost_id = -1; - bool found_route = false; - - const gd::Polygon *reachable_end = nullptr; - real_t distance_to_reachable_end = FLT_MAX; - bool is_reachable = true; - - while (true) { - // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. - for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) { - // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. - for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { - const gd::Edge::Connection &connection = edge.connections[connection_index]; - - // Only consider the connection to another polygon if this polygon is in a region with compatible layers. - if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { - continue; - } - - const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; - real_t poly_enter_cost = 0.0; - real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); - - if (prev_least_cost_id != -1 && navigation_polys[prev_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(); - } - prev_least_cost_id = least_cost_id; - - 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 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. - gd::NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id]; - if (neighbor_poly.poly != nullptr) { - // If the neighbor polygon hasn't been traversed yet and the new path leading to - // it is shorter, update the polygon. - if (neighbor_poly.traversable_poly_index < traversable_polys.size() && - new_traveled_distance < neighbor_poly.traveled_distance) { - 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) * - neighbor_poly.poly->owner->get_travel_cost(); - neighbor_poly.entry = new_entry; - - // Update the priority of the polygon in the heap. - traversable_polys.shift(neighbor_poly.traversable_poly_index); - } - } else { - // Initialize the matching navigation polygon. - neighbor_poly.poly = connection.polygon; - 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) * - neighbor_poly.poly->owner->get_travel_cost(); - neighbor_poly.entry = new_entry; - - // Add the polygon to the heap of polygons to traverse next. - traversable_polys.push(&neighbor_poly); - } - } - } - - // 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; - end_d = FLT_MAX; - for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { - Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - 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 (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - 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. - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - for (gd::NavigationPoly &nav_poly : navigation_polys) { - nav_poly.poly = nullptr; - } - navigation_polys[begin_poly->id].poly = begin_poly; - - least_cost_id = begin_poly->id; - prev_least_cost_id = -1; - - reachable_end = nullptr; - - continue; - } - - // 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_to(p_destination); - 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; - } - } - - // 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) { - end_d = FLT_MAX; - // Search all faces of the start polygon for the closest point to our target position. - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - } - } - - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - Vector path; - // Optimize the path. - if (p_optimize) { - // Set the apex poly/point to the end point - gd::NavigationPoly *apex_poly = &navigation_polys[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(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 = &navigation_polys[apex_poly->back_navigation_poly_id]; - } - } - - Vector3 apex_point = end_point; - - gd::NavigationPoly *left_poly = apex_poly; - Vector3 left_portal = apex_point; - gd::NavigationPoly *right_poly = apex_poly; - Vector3 right_portal = apex_point; - - gd::NavigationPoly *p = apex_poly; - - path.push_back(end_point); - APPEND_METADATA(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(up) < 0) { - SWAP(left, right); - } - - bool skip = false; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) { - //process - if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) { - left_poly = p; - left_portal = left; - } else { - clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners); - - apex_point = right_portal; - p = right_poly; - left_poly = p; - apex_poly = p; - left_portal = apex_point; - right_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - skip = true; - } - } - - if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) { - //process - if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) { - right_poly = p; - right_portal = right; - } else { - clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners); - - apex_point = left_portal; - p = left_poly; - right_poly = p; - apex_poly = p; - right_portal = apex_point; - left_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(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 (path[path.size() - 1] != begin_point) { - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - } - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); - } - - } else { - path.push_back(end_point); - APPEND_METADATA(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->points.size(); - Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; - - path.push_back(point); - APPEND_METADATA(navigation_polys[np_id].poly); - } else { - path.push_back(navigation_polys[np_id].entry); - APPEND_METADATA(navigation_polys[np_id].poly); - } - - np_id = navigation_polys[np_id].back_navigation_poly_id; - } - - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); - } - } - - // Ensure post conditions (path arrays MUST match in size). - CRASH_COND(r_path_types && path.size() != r_path_types->size()); - CRASH_COND(r_path_rids && path.size() != r_path_rids->size()); - CRASH_COND(r_path_owners && path.size() != r_path_owners->size()); - - return path; + return NavMeshQueries3D::polygons_get_path( + polygons, p_origin, p_destination, p_optimize, p_navigation_layers, + r_path_types, r_path_rids, r_path_owners, up, link_polygons.size()); } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -600,66 +142,7 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector return Vector3(); } - bool use_collision = p_use_collision; - Vector3 closest_point; - real_t closest_point_d = FLT_MAX; - - for (const gd::Polygon &p : polygons) { - // For each face check the distance to the segment - for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { - const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - Vector3 inters; - if (f.intersects_segment(p_from, p_to, &inters)) { - const real_t d = p_from.distance_to(inters); - if (use_collision == false) { - closest_point = inters; - use_collision = true; - closest_point_d = d; - } else if (closest_point_d > d) { - closest_point = inters; - closest_point_d = d; - } - } - // If segment does not itersect face, check the distance from segment's endpoints. - else if (!use_collision) { - const Vector3 p_from_closest = f.get_closest_point_to(p_from); - const real_t d_p_from = p_from.distance_to(p_from_closest); - if (closest_point_d > d_p_from) { - closest_point = p_from_closest; - closest_point_d = d_p_from; - } - - const Vector3 p_to_closest = f.get_closest_point_to(p_to); - const real_t d_p_to = p_to.distance_to(p_to_closest); - if (closest_point_d > d_p_to) { - closest_point = p_to_closest; - closest_point_d = d_p_to; - } - } - } - // Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment. - if (!use_collision) { - for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) { - Vector3 a, b; - - Geometry3D::get_closest_points_between_segments( - p_from, - p_to, - p.points[point_id].pos, - p.points[(point_id + 1) % p.points.size()].pos, - a, - b); - - const real_t d = a.distance_to(b); - if (d < closest_point_d) { - closest_point_d = d; - closest_point = b; - } - } - } - } - - return closest_point; + return NavMeshQueries3D::polygons_get_closest_point_to_segment(polygons, p_from, p_to, p_use_collision); } Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { @@ -668,8 +151,8 @@ Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.point; + + return NavMeshQueries3D::polygons_get_closest_point(polygons, p_point); } Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { @@ -678,8 +161,8 @@ Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.normal; + + return NavMeshQueries3D::polygons_get_closest_point_normal(polygons, p_point); } RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { @@ -688,32 +171,14 @@ RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return RID(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.owner; + + return NavMeshQueries3D::polygons_get_closest_point_owner(polygons, p_point); } gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const { RWLockRead read_lock(map_rwlock); - gd::ClosestPointQueryResult result; - real_t closest_point_ds = FLT_MAX; - - for (const gd::Polygon &p : polygons) { - // For each face check the distance to the point - for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { - const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - const Vector3 inters = f.get_closest_point_to(p_point); - const real_t ds = inters.distance_squared_to(p_point); - if (ds < closest_point_ds) { - result.point = inters; - result.normal = f.get_plane().normal; - result.owner = p.owner->get_self(); - closest_point_ds = ds; - } - } - } - - return result; + return NavMeshQueries3D::polygons_get_closest_point_info(polygons, p_point); } void NavMap::add_region(NavRegion *p_region) { @@ -1386,39 +851,6 @@ void NavMap::dispatch_callbacks() { } } -void NavMap::clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners) const { - Vector3 from = path[path.size() - 1]; - - if (from.is_equal_approx(p_to_point)) { - return; - } - Plane cut_plane; - cut_plane.normal = (from - p_to_point).cross(up); - if (cut_plane.normal == Vector3()) { - return; - } - cut_plane.normal.normalize(); - cut_plane.d = cut_plane.normal.dot(from); - - while (from_poly != p_to_poly) { - Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start; - 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]; - - if (!pathway_start.is_equal_approx(pathway_end)) { - Vector3 inters; - if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { - if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) { - path.push_back(inters); - APPEND_METADATA(from_poly->poly); - } - } - } - } -} - void NavMap::_update_merge_rasterizer_cell_dimensions() { merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale; merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale; diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h index c18ee7155b2..b9120c04d9a 100644 --- a/modules/navigation/nav_map.h +++ b/modules/navigation/nav_map.h @@ -232,7 +232,6 @@ private: void compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent); void compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent); - void clip_path(const LocalVector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector *r_path_types, TypedArray *r_path_rids, Vector *r_path_owners) const; void _update_rvo_simulation(); void _update_rvo_obstacles_tree_2d(); void _update_rvo_agents_tree_2d(); diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp index 85510bd4163..7a44adecbc3 100644 --- a/modules/navigation/nav_region.cpp +++ b/modules/navigation/nav_region.cpp @@ -32,6 +32,8 @@ #include "nav_map.h" +#include "3d/nav_mesh_queries_3d.h" + void NavRegion::set_map(NavMap *p_map) { if (map == p_map) { return; @@ -108,81 +110,7 @@ Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniform return Vector3(); } - const LocalVector ®ion_polygons = get_polygons(); - - if (region_polygons.is_empty()) { - return Vector3(); - } - - 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 gd::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 Vector3(); - } - - 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, Vector3()); - uint32_t rrp_polygon_index = region_E->value; - ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector3()); - - const gd::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.points.size(); rpp_index++) { - real_t face_area = Face3(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area(); - - if (face_area == 0.0) { - continue; - } - polygon_area_map[accumulated_polygon_area] = rpp_index; - accumulated_polygon_area += face_area; - } - if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) { - // All faces have no real surface / no area. - return Vector3(); - } - - 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, Vector3()); - uint32_t rrp_face_index = polygon_E->value; - ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector3()); - - const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); - - Vector3 face_random_position = face.get_random_point_inside(); - return face_random_position; - - } else { - uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1); - - const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index]; - - uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1); - - const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos); - - Vector3 face_random_position = face.get_random_point_inside(); - return face_random_position; - } + return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly); } bool NavRegion::sync() {