summaryrefslogtreecommitdiffstats
path: root/modules/navigation
diff options
context:
space:
mode:
authorsmix8 <52464204+smix8@users.noreply.github.com>2024-08-30 15:52:29 +0200
committersmix8 <52464204+smix8@users.noreply.github.com>2024-09-03 13:16:35 +0200
commita4cfc77dc070d93840c186509d795b17b635bf93 (patch)
treec69ad687351d771ca22fa32a1fea51463cfd89f8 /modules/navigation
parent8120e0324a48190f58616378746d517e4ad40965 (diff)
downloadredot-engine-a4cfc77dc070d93840c186509d795b17b635bf93.tar.gz
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.
Diffstat (limited to 'modules/navigation')
-rw-r--r--modules/navigation/3d/nav_mesh_queries_3d.cpp715
-rw-r--r--modules/navigation/3d/nav_mesh_queries_3d.h54
-rw-r--r--modules/navigation/nav_map.cpp594
-rw-r--r--modules/navigation/nav_map.h1
-rw-r--r--modules/navigation/nav_region.cpp78
5 files changed, 785 insertions, 657 deletions
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 0000000000..70207f86ce
--- /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<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
+ const LocalVector<gd::Polygon> &region_polygons = p_polygons;
+
+ if (region_polygons.is_empty()) {
+ return Vector3();
+ }
+
+ if (p_uniformly) {
+ real_t accumulated_area = 0;
+ RBMap<real_t, uint32_t> region_area_map;
+
+ for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
+ const gd::Polygon &region_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<real_t, uint32_t>::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<real_t, uint32_t> 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<real_t, uint32_t>::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<Vector3> NavMeshQueries3D::polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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<Vector3>();
+ }
+ 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<Vector3> path;
+ path.resize(2);
+ path.write[0] = begin_point;
+ path.write[1] = end_point;
+ return path;
+ }
+
+ // List of all reachable navigation polys.
+ LocalVector<gd::NavigationPoly> 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<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
+ 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<Vector3> 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<Vector3> path;
+ path.resize(2);
+ path.write[0] = begin_point;
+ path.write[1] = end_point;
+ return path;
+ }
+
+ Vector<Vector3> 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<gd::Polygon> &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<gd::Polygon> &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<gd::Polygon> &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<gd::Polygon> &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<gd::Polygon> &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<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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 0000000000..109bb2f971
--- /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<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
+
+ static Vector<Vector3> polygons_get_path(const LocalVector<gd::Polygon> &p_polygons, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners, const Vector3 &p_map_up, uint32_t p_link_polygons_size);
+ static Vector3 polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
+ static Vector3 polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
+ static Vector3 polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
+ static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
+ static RID polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
+
+ static void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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 f89f5b5812..dd77e81b45 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 <Obstacle2d.h>
-#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<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
return Vector<Vector3>();
}
- // 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<Vector3>();
- }
- 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<Vector3> path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
- }
-
- // List of all reachable navigation polys.
- LocalVector<gd::NavigationPoly> 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<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
- 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<Vector3> 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<Vector3> path;
- path.resize(2);
- path.write[0] = begin_point;
- path.write[1] = end_point;
- return path;
- }
-
- Vector<Vector3> 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<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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 c18ee7155b..b9120c04d9 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<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *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 85510bd416..7a44adecbc 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<gd::Polygon> &region_polygons = get_polygons();
-
- if (region_polygons.is_empty()) {
- return Vector3();
- }
-
- if (p_uniformly) {
- real_t accumulated_area = 0;
- RBMap<real_t, uint32_t> region_area_map;
-
- for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
- const gd::Polygon &region_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<real_t, uint32_t>::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<real_t, uint32_t> 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<real_t, uint32_t>::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() {