mirror of
https://github.com/godotengine/godot.git
synced 2025-01-22 10:32:54 -05:00
Patch navigation map async synchronization
Patches navigation map async synchronization.
This commit is contained in:
smix8
parent
0f95e9f8e6
commit
c69408168c
12 changed files with 411 additions and 413 deletions
|
|
@ -43,6 +43,15 @@ struct NavBaseIteration {
|
|||
ObjectID owner_object_id;
|
||||
RID owner_rid;
|
||||
bool owner_use_edge_connections = false;
|
||||
|
||||
bool get_enabled() const { return enabled; }
|
||||
NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
|
||||
RID get_self() const { return owner_rid; }
|
||||
ObjectID get_owner_id() const { return owner_object_id; }
|
||||
uint32_t get_navigation_layers() const { return navigation_layers; }
|
||||
real_t get_enter_cost() const { return enter_cost; }
|
||||
real_t get_travel_cost() const { return travel_cost; }
|
||||
bool get_use_edge_connections() const { return owner_use_edge_connections; }
|
||||
};
|
||||
|
||||
#endif // NAV_BASE_ITERATION_3D_H
|
||||
|
|
|
|||
|
|
@ -52,6 +52,14 @@ gd::PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3
|
|||
}
|
||||
|
||||
void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
|
||||
gd::PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
performance_data.pm_polygon_count = 0;
|
||||
performance_data.pm_edge_count = 0;
|
||||
performance_data.pm_edge_merge_count = 0;
|
||||
performance_data.pm_edge_connection_count = 0;
|
||||
performance_data.pm_edge_free_count = 0;
|
||||
|
||||
_build_step_gather_region_polygons(r_build);
|
||||
|
||||
_build_step_find_edge_connection_pairs(r_build);
|
||||
|
|
@ -66,89 +74,108 @@ void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
|
|||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r_build) {
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
gd::PerformanceData &performance_data = r_build.performance_data;
|
||||
int polygon_count = r_build.polygon_count;
|
||||
int navmesh_polygon_count = r_build.navmesh_polygon_count;
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
|
||||
LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
|
||||
LocalVector<NavRegionIteration> ®ions = map_iteration->region_iterations;
|
||||
HashMap<uint32_t, LocalVector<gd::Edge::Connection>> ®ion_external_connections = map_iteration->external_region_connections;
|
||||
|
||||
// Remove regions connections.
|
||||
map_iteration->external_region_connections.clear();
|
||||
for (const NavRegionIteration ®ion : map_iteration->region_iterations) {
|
||||
map_iteration->external_region_connections[region.id] = LocalVector<gd::Edge::Connection>();
|
||||
region_external_connections.clear();
|
||||
for (const NavRegionIteration ®ion : regions) {
|
||||
region_external_connections[region.id] = LocalVector<gd::Edge::Connection>();
|
||||
}
|
||||
|
||||
polygon_count = 0;
|
||||
navmesh_polygon_count = 0;
|
||||
for (NavRegionIteration ®ion : map_iteration->region_iterations) {
|
||||
for (gd::Polygon ®ion_polygon : region.navmesh_polygons) {
|
||||
region_polygon.id = polygon_count;
|
||||
region_polygon.owner = ®ion;
|
||||
// Resize the polygon count.
|
||||
int polygon_count = 0;
|
||||
for (const NavRegionIteration ®ion : regions) {
|
||||
if (!region.get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
polygon_count += region.get_navmesh_polygons().size();
|
||||
}
|
||||
polygons.resize(polygon_count);
|
||||
|
||||
// Copy all region polygons in the map.
|
||||
polygon_count = 0;
|
||||
for (const NavRegionIteration ®ion : regions) {
|
||||
if (!region.get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
const LocalVector<gd::Polygon> &polygons_source = region.get_navmesh_polygons();
|
||||
for (uint32_t n = 0; n < polygons_source.size(); n++) {
|
||||
polygons[polygon_count] = polygons_source[n];
|
||||
polygons[polygon_count].id = polygon_count;
|
||||
polygon_count++;
|
||||
navmesh_polygon_count++;
|
||||
}
|
||||
}
|
||||
|
||||
performance_data.pm_polygon_count = polygon_count;
|
||||
r_build.polygon_count = polygon_count;
|
||||
r_build.navmesh_polygon_count = navmesh_polygon_count;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build) {
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &iter_connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
gd::PerformanceData &performance_data = r_build.performance_data;
|
||||
int free_edge_count = r_build.free_edge_count;
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
|
||||
iter_connection_pairs_map.clear();
|
||||
iter_connection_pairs_map.reserve(map_iteration->region_iterations.size());
|
||||
LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
|
||||
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
|
||||
for (NavRegionIteration ®ion : map_iteration->region_iterations) {
|
||||
for (gd::Polygon ®ion_polygon : region.navmesh_polygons) {
|
||||
for (uint32_t p = 0; p < region_polygon.points.size(); p++) {
|
||||
const int next_point = (p + 1) % region_polygon.points.size();
|
||||
const gd::EdgeKey ek(region_polygon.points[p].key, region_polygon.points[next_point].key);
|
||||
// Group all edges per key.
|
||||
connection_pairs_map.clear();
|
||||
connection_pairs_map.reserve(polygons.size());
|
||||
int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
|
||||
|
||||
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey>::Iterator pair_it = iter_connection_pairs_map.find(ek);
|
||||
if (!pair_it) {
|
||||
pair_it = iter_connection_pairs_map.insert(ek, gd::EdgeConnectionPair());
|
||||
performance_data.pm_edge_count += 1;
|
||||
++free_edge_count;
|
||||
for (gd::Polygon &poly : polygons) {
|
||||
for (uint32_t p = 0; p < poly.points.size(); p++) {
|
||||
const int next_point = (p + 1) % poly.points.size();
|
||||
const gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
|
||||
|
||||
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
|
||||
if (!pair_it) {
|
||||
pair_it = connection_pairs_map.insert(ek, gd::EdgeConnectionPair());
|
||||
performance_data.pm_edge_count += 1;
|
||||
++free_edges_count;
|
||||
}
|
||||
gd::EdgeConnectionPair &pair = pair_it->value;
|
||||
if (pair.size < 2) {
|
||||
// Add the polygon/edge tuple to this key.
|
||||
gd::Edge::Connection new_connection;
|
||||
new_connection.polygon = &poly;
|
||||
new_connection.edge = p;
|
||||
new_connection.pathway_start = poly.points[p].pos;
|
||||
new_connection.pathway_end = poly.points[next_point].pos;
|
||||
|
||||
pair.connections[pair.size] = new_connection;
|
||||
++pair.size;
|
||||
if (pair.size == 2) {
|
||||
--free_edges_count;
|
||||
}
|
||||
gd::EdgeConnectionPair &pair = pair_it->value;
|
||||
if (pair.size < 2) {
|
||||
pair.connections[pair.size].polygon = ®ion_polygon;
|
||||
pair.connections[pair.size].edge = p;
|
||||
pair.connections[pair.size].pathway_start = region_polygon.points[p].pos;
|
||||
pair.connections[pair.size].pathway_end = region_polygon.points[next_point].pos;
|
||||
++pair.size;
|
||||
if (pair.size == 2) {
|
||||
--free_edge_count;
|
||||
}
|
||||
|
||||
} else {
|
||||
// The edge is already connected with another edge, skip.
|
||||
ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
|
||||
}
|
||||
} else {
|
||||
// The edge is already connected with another edge, skip.
|
||||
ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
|
||||
}
|
||||
}
|
||||
}
|
||||
r_build.free_edge_count = free_edge_count;
|
||||
|
||||
r_build.free_edge_count = free_edges_count;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build) {
|
||||
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &iter_connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
LocalVector<gd::Edge::Connection> &iter_free_edges = r_build.iter_free_edges;
|
||||
bool use_edge_connections = r_build.use_edge_connections;
|
||||
gd::PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
iter_free_edges.clear();
|
||||
iter_free_edges.resize(r_build.free_edge_count);
|
||||
uint32_t iter_free_edge_index = 0;
|
||||
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
LocalVector<gd::Edge::Connection> &free_edges = r_build.iter_free_edges;
|
||||
int free_edges_count = r_build.free_edge_count;
|
||||
bool use_edge_connections = r_build.use_edge_connections;
|
||||
|
||||
for (const KeyValue<gd::EdgeKey, gd::EdgeConnectionPair> &pair_it : iter_connection_pairs_map) {
|
||||
free_edges.clear();
|
||||
free_edges.reserve(free_edges_count);
|
||||
|
||||
for (const KeyValue<gd::EdgeKey, gd::EdgeConnectionPair> &pair_it : connection_pairs_map) {
|
||||
const gd::EdgeConnectionPair &pair = pair_it.value;
|
||||
|
||||
if (pair.size == 2) {
|
||||
// Connect edge that are shared in different polygons.
|
||||
const gd::Edge::Connection &c1 = pair.connections[0];
|
||||
|
|
@ -159,21 +186,21 @@ void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBui
|
|||
performance_data.pm_edge_merge_count += 1;
|
||||
} else {
|
||||
CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
|
||||
if (use_edge_connections && pair.connections[0].polygon->owner->owner_use_edge_connections) {
|
||||
iter_free_edges[iter_free_edge_index++] = pair.connections[0];
|
||||
if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
|
||||
free_edges.push_back(pair.connections[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
iter_free_edges.resize(iter_free_edge_index);
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build) {
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
const LocalVector<gd::Edge::Connection> &iter_free_edges = r_build.iter_free_edges;
|
||||
bool use_edge_connections = r_build.use_edge_connections;
|
||||
gd::PerformanceData &performance_data = r_build.performance_data;
|
||||
const real_t edge_connection_margin = r_build.edge_connection_margin;
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
|
||||
real_t edge_connection_margin = r_build.edge_connection_margin;
|
||||
LocalVector<gd::Edge::Connection> &free_edges = r_build.iter_free_edges;
|
||||
HashMap<uint32_t, LocalVector<gd::Edge::Connection>> ®ion_external_connections = map_iteration->external_region_connections;
|
||||
|
||||
// Find the compatible near edges.
|
||||
//
|
||||
// Note:
|
||||
|
|
@ -181,26 +208,17 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
|
|||
// to be connected, create new polygons to remove that small gap is
|
||||
// not really useful and would result in wasteful computation during
|
||||
// connection, integration and path finding.
|
||||
|
||||
performance_data.pm_edge_free_count = iter_free_edges.size();
|
||||
|
||||
if (!use_edge_connections) {
|
||||
return;
|
||||
}
|
||||
performance_data.pm_edge_free_count = free_edges.size();
|
||||
|
||||
const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
|
||||
|
||||
for (uint32_t i = 0; i < iter_free_edges.size(); i++) {
|
||||
const gd::Edge::Connection &free_edge = iter_free_edges[i];
|
||||
|
||||
for (uint32_t i = 0; i < free_edges.size(); i++) {
|
||||
const gd::Edge::Connection &free_edge = free_edges[i];
|
||||
Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
|
||||
Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
|
||||
|
||||
Vector3 edge_vector = edge_p2 - edge_p1;
|
||||
real_t edge_vector_length_squared = edge_vector.length_squared();
|
||||
|
||||
for (uint32_t j = 0; j < iter_free_edges.size(); j++) {
|
||||
const gd::Edge::Connection &other_edge = iter_free_edges[j];
|
||||
for (uint32_t j = 0; j < free_edges.size(); j++) {
|
||||
const gd::Edge::Connection &other_edge = free_edges[j];
|
||||
if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
|
||||
continue;
|
||||
}
|
||||
|
|
@ -209,8 +227,9 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
|
|||
Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
|
||||
|
||||
// Compute the projection of the opposite edge on the current one
|
||||
real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector_length_squared);
|
||||
real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector_length_squared);
|
||||
Vector3 edge_vector = edge_p2 - edge_p1;
|
||||
real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
|
||||
real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
|
||||
if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
|
||||
continue;
|
||||
}
|
||||
|
|
@ -245,7 +264,7 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
|
|||
free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
|
||||
|
||||
// Add the connection to the region_connection map.
|
||||
map_iteration->external_region_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
|
||||
region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
|
||||
performance_data.pm_edge_connection_count += 1;
|
||||
}
|
||||
}
|
||||
|
|
@ -253,19 +272,26 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
|
|||
|
||||
void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_build) {
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
const Vector3 &merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
|
||||
|
||||
real_t link_connection_radius = r_build.link_connection_radius;
|
||||
real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
|
||||
Vector3 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
|
||||
|
||||
LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
|
||||
LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
|
||||
LocalVector<NavLinkIteration> &links = map_iteration->link_iterations;
|
||||
int polygon_count = r_build.polygon_count;
|
||||
int link_polygon_count = r_build.link_polygon_count;
|
||||
|
||||
real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
|
||||
uint32_t link_poly_idx = 0;
|
||||
link_polygons.resize(links.size());
|
||||
|
||||
// Search for polygons within range of a nav link.
|
||||
for (NavLinkIteration &link : map_iteration->link_iterations) {
|
||||
if (!link.enabled) {
|
||||
for (const NavLinkIteration &link : links) {
|
||||
if (!link.get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
const Vector3 link_start_pos = link.start_position;
|
||||
const Vector3 link_end_pos = link.end_position;
|
||||
const Vector3 link_start_pos = link.get_start_position();
|
||||
const Vector3 link_end_pos = link.get_end_position();
|
||||
|
||||
gd::Polygon *closest_start_polygon = nullptr;
|
||||
real_t closest_start_sqr_dist = link_connection_radius_sqr;
|
||||
|
|
@ -275,37 +301,31 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
|
|||
real_t closest_end_sqr_dist = link_connection_radius_sqr;
|
||||
Vector3 closest_end_point;
|
||||
|
||||
for (NavRegionIteration ®ion : map_iteration->region_iterations) {
|
||||
AABB region_bounds = region.bounds.grow(link_connection_radius);
|
||||
if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
|
||||
continue;
|
||||
}
|
||||
for (gd::Polygon &polyon : region.navmesh_polygons) {
|
||||
for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) {
|
||||
const Face3 face(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos);
|
||||
for (gd::Polygon &polyon : polygons) {
|
||||
for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) {
|
||||
const Face3 face(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos);
|
||||
|
||||
{
|
||||
const Vector3 start_point = face.get_closest_point_to(link_start_pos);
|
||||
const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
|
||||
{
|
||||
const Vector3 start_point = face.get_closest_point_to(link_start_pos);
|
||||
const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
|
||||
|
||||
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
|
||||
if (sqr_dist < closest_start_sqr_dist) {
|
||||
closest_start_sqr_dist = sqr_dist;
|
||||
closest_start_point = start_point;
|
||||
closest_start_polygon = &polyon;
|
||||
}
|
||||
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
|
||||
if (sqr_dist < closest_start_sqr_dist) {
|
||||
closest_start_sqr_dist = sqr_dist;
|
||||
closest_start_point = start_point;
|
||||
closest_start_polygon = &polyon;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const Vector3 end_point = face.get_closest_point_to(link_end_pos);
|
||||
const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
|
||||
{
|
||||
const Vector3 end_point = face.get_closest_point_to(link_end_pos);
|
||||
const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
|
||||
|
||||
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
|
||||
if (sqr_dist < closest_end_sqr_dist) {
|
||||
closest_end_sqr_dist = sqr_dist;
|
||||
closest_end_point = end_point;
|
||||
closest_end_polygon = &polyon;
|
||||
}
|
||||
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
|
||||
if (sqr_dist < closest_end_sqr_dist) {
|
||||
closest_end_sqr_dist = sqr_dist;
|
||||
closest_end_point = end_point;
|
||||
closest_end_polygon = &polyon;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -313,13 +333,10 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
|
|||
|
||||
// If we have both a start and end point, then create a synthetic polygon to route through.
|
||||
if (closest_start_polygon && closest_end_polygon) {
|
||||
link.navmesh_polygons.resize(1);
|
||||
gd::Polygon &new_polygon = link.navmesh_polygons[0];
|
||||
gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
|
||||
new_polygon.id = polygon_count++;
|
||||
new_polygon.owner = &link;
|
||||
|
||||
link_polygon_count++;
|
||||
|
||||
new_polygon.edges.clear();
|
||||
new_polygon.edges.resize(4);
|
||||
new_polygon.points.resize(4);
|
||||
|
|
@ -348,7 +365,7 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
|
|||
}
|
||||
|
||||
// If the link is bi-directional, create connections from the end to the start.
|
||||
if (link.bidirectional) {
|
||||
if (link.is_bidirectional()) {
|
||||
gd::Edge::Connection entry_connection;
|
||||
entry_connection.polygon = &new_polygon;
|
||||
entry_connection.edge = -1;
|
||||
|
|
@ -365,33 +382,23 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
r_build.polygon_count = polygon_count;
|
||||
r_build.link_polygon_count = link_polygon_count;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild &r_build) {
|
||||
NavMapIteration *map_iteration = r_build.map_iteration;
|
||||
|
||||
map_iteration->navmesh_polygon_count = r_build.navmesh_polygon_count;
|
||||
map_iteration->link_polygon_count = r_build.link_polygon_count;
|
||||
LocalVector<gd::Polygon> &polygons = map_iteration->navmesh_polygons;
|
||||
LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
|
||||
|
||||
// TODO: This copying is for compatibility with legacy functions that expect a big polygon soup array.
|
||||
// Those functions should be changed to work hierarchical with the region iteration polygons directly.
|
||||
map_iteration->navmesh_polygons.resize(map_iteration->navmesh_polygon_count);
|
||||
uint32_t polygon_index = 0;
|
||||
for (NavRegionIteration ®ion : map_iteration->region_iterations) {
|
||||
for (gd::Polygon ®ion_polygon : region.navmesh_polygons) {
|
||||
map_iteration->navmesh_polygons[polygon_index++] = region_polygon;
|
||||
}
|
||||
}
|
||||
map_iteration->navmesh_polygon_count = polygons.size();
|
||||
map_iteration->link_polygon_count = link_polygons.size();
|
||||
|
||||
map_iteration->path_query_slots_mutex.lock();
|
||||
for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
|
||||
p_path_query_slot.path_corridor.clear();
|
||||
p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
|
||||
p_path_query_slot.traversable_polys.clear();
|
||||
p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25);
|
||||
p_path_query_slot.path_corridor.clear();
|
||||
p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
|
||||
}
|
||||
map_iteration->path_query_slots_mutex.unlock();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -79,6 +79,7 @@ struct NavMapIteration {
|
|||
|
||||
Vector3 map_up;
|
||||
LocalVector<gd::Polygon> navmesh_polygons;
|
||||
LocalVector<gd::Polygon> link_polygons;
|
||||
|
||||
LocalVector<NavRegionIteration> region_iterations;
|
||||
LocalVector<NavLinkIteration> link_iterations;
|
||||
|
|
|
|||
|
|
@ -129,41 +129,17 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
|
|||
}
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *p_begin_polygon, const gd::Polygon *p_end_polygon) {
|
||||
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
|
||||
p_query_task.path_meta_point_types.resize(2);
|
||||
p_query_task.path_meta_point_types[0] = p_begin_polygon->owner->owner_type;
|
||||
p_query_task.path_meta_point_types[1] = p_end_polygon->owner->owner_type;
|
||||
}
|
||||
|
||||
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
|
||||
p_query_task.path_meta_point_rids.resize(2);
|
||||
p_query_task.path_meta_point_rids[0] = p_begin_polygon->owner->owner_rid;
|
||||
p_query_task.path_meta_point_rids[1] = p_end_polygon->owner->owner_rid;
|
||||
}
|
||||
|
||||
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
|
||||
p_query_task.path_meta_point_owners.resize(2);
|
||||
p_query_task.path_meta_point_owners[0] = p_begin_polygon->owner->owner_object_id;
|
||||
p_query_task.path_meta_point_owners[1] = p_end_polygon->owner->owner_object_id;
|
||||
}
|
||||
|
||||
p_query_task.path_points.resize(2);
|
||||
p_query_task.path_points[0] = p_query_task.begin_position;
|
||||
p_query_task.path_points[1] = p_query_task.end_position;
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon) {
|
||||
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
|
||||
p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->owner_type);
|
||||
p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
|
||||
}
|
||||
|
||||
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
|
||||
p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->owner_rid);
|
||||
p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
|
||||
}
|
||||
|
||||
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
|
||||
p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->owner_object_id);
|
||||
p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
|
||||
}
|
||||
|
||||
p_query_task.path_points.push_back(p_point);
|
||||
|
|
@ -215,49 +191,11 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQuery
|
|||
|
||||
map->query_path(query_task);
|
||||
|
||||
const uint32_t path_point_size = query_task.path_points.size();
|
||||
|
||||
Vector<Vector3> path_points;
|
||||
Vector<int32_t> path_meta_point_types;
|
||||
TypedArray<RID> path_meta_point_rids;
|
||||
Vector<int64_t> path_meta_point_owners;
|
||||
|
||||
{
|
||||
path_points.resize(path_point_size);
|
||||
Vector3 *w = path_points.ptrw();
|
||||
const Vector3 *r = query_task.path_points.ptr();
|
||||
for (uint32_t i = 0; i < path_point_size; i++) {
|
||||
w[i] = r[i];
|
||||
}
|
||||
}
|
||||
|
||||
if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
|
||||
path_meta_point_types.resize(path_point_size);
|
||||
int32_t *w = path_meta_point_types.ptrw();
|
||||
const int32_t *r = query_task.path_meta_point_types.ptr();
|
||||
for (uint32_t i = 0; i < path_point_size; i++) {
|
||||
w[i] = r[i];
|
||||
}
|
||||
}
|
||||
if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
|
||||
path_meta_point_rids.resize(path_point_size);
|
||||
for (uint32_t i = 0; i < path_point_size; i++) {
|
||||
path_meta_point_rids[i] = query_task.path_meta_point_rids[i];
|
||||
}
|
||||
}
|
||||
if (query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
|
||||
path_meta_point_owners.resize(path_point_size);
|
||||
int64_t *w = path_meta_point_owners.ptrw();
|
||||
const int64_t *r = query_task.path_meta_point_owners.ptr();
|
||||
for (uint32_t i = 0; i < path_point_size; i++) {
|
||||
w[i] = r[i];
|
||||
}
|
||||
}
|
||||
|
||||
p_query_result->set_path(path_points);
|
||||
p_query_result->set_path_types(path_meta_point_types);
|
||||
p_query_result->set_path_rids(path_meta_point_rids);
|
||||
p_query_result->set_path_owner_ids(path_meta_point_owners);
|
||||
p_query_result->set_data(
|
||||
query_task.path_points,
|
||||
query_task.path_meta_point_types,
|
||||
query_task.path_meta_point_rids,
|
||||
query_task.path_meta_point_owners);
|
||||
|
||||
if (query_task.callback.is_valid()) {
|
||||
if (emit_callback(query_task.callback)) {
|
||||
|
|
@ -269,25 +207,23 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQuery
|
|||
}
|
||||
|
||||
void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
|
||||
p_query_task.path_points.clear();
|
||||
p_query_task.path_meta_point_types.clear();
|
||||
p_query_task.path_meta_point_rids.clear();
|
||||
p_query_task.path_meta_point_owners.clear();
|
||||
p_query_task.path_clear();
|
||||
|
||||
_query_task_find_start_end_positions(p_query_task, p_polygons);
|
||||
|
||||
// Check for trivial cases
|
||||
// Check for trivial cases.
|
||||
if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
|
||||
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED;
|
||||
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
||||
return;
|
||||
}
|
||||
|
||||
if (p_query_task.begin_polygon == p_query_task.end_polygon) {
|
||||
_query_task_create_same_polygon_two_point_path(p_query_task, p_query_task.begin_polygon, p_query_task.end_polygon);
|
||||
p_query_task.path_clear();
|
||||
_query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon);
|
||||
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
||||
return;
|
||||
}
|
||||
|
||||
DEV_ASSERT(p_query_task.path_query_slot->path_corridor.size() == p_polygons.size() + p_query_task.link_polygons_size);
|
||||
_query_task_build_path_corridor(p_query_task, p_polygons);
|
||||
|
||||
if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
|
||||
|
|
@ -311,10 +247,7 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
|
|||
} break;
|
||||
}
|
||||
|
||||
p_query_task.path_points.invert();
|
||||
p_query_task.path_meta_point_types.invert();
|
||||
p_query_task.path_meta_point_rids.invert();
|
||||
p_query_task.path_meta_point_owners.invert();
|
||||
p_query_task.path_reverse();
|
||||
|
||||
if (p_query_task.simplify_path) {
|
||||
_query_task_simplified_path_points(p_query_task);
|
||||
|
|
@ -339,33 +272,32 @@ void NavMeshQueries3D::query_task_polygons_get_path(NavMeshPathQueryTask3D &p_qu
|
|||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
|
||||
const gd::Polygon *begin_polygon = p_query_task.begin_polygon;
|
||||
const gd::Polygon *end_polygon = p_query_task.end_polygon;
|
||||
const Vector3 &begin_position = p_query_task.begin_position;
|
||||
Vector3 &end_position = p_query_task.end_position;
|
||||
// List of all reachable navigation polys.
|
||||
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
|
||||
for (gd::NavigationPoly &polygon : navigation_polys) {
|
||||
polygon.reset();
|
||||
}
|
||||
|
||||
DEV_ASSERT(navigation_polys.size() == p_polygons.size() + p_query_task.link_polygons_size);
|
||||
|
||||
// Initialize the matching navigation polygon.
|
||||
gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_polygon->id];
|
||||
begin_navigation_poly.poly = begin_polygon;
|
||||
begin_navigation_poly.entry = begin_position;
|
||||
begin_navigation_poly.back_navigation_edge_pathway_start = begin_position;
|
||||
begin_navigation_poly.back_navigation_edge_pathway_end = begin_position;
|
||||
const Vector3 p_target_position = p_query_task.target_position;
|
||||
const uint32_t p_navigation_layers = p_query_task.navigation_layers;
|
||||
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
|
||||
const gd::Polygon *end_poly = p_query_task.end_polygon;
|
||||
Vector3 begin_point = p_query_task.begin_position;
|
||||
Vector3 end_point = p_query_task.end_position;
|
||||
|
||||
// Heap of polygons to travel next.
|
||||
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
|
||||
&traversable_polys = p_query_task.path_query_slot->traversable_polys;
|
||||
traversable_polys.clear();
|
||||
traversable_polys.reserve(p_polygons.size() * 0.25);
|
||||
|
||||
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
|
||||
for (gd::NavigationPoly &polygon : navigation_polys) {
|
||||
polygon.reset();
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
// This is an implementation of the A* algorithm.
|
||||
p_query_task.least_cost_id = begin_polygon->id;
|
||||
int least_cost_id = begin_poly->id;
|
||||
int prev_least_cost_id = -1;
|
||||
bool found_route = false;
|
||||
|
||||
|
|
@ -375,24 +307,24 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
|
||||
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[p_query_task.least_cost_id].poly->edges) {
|
||||
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 (uint32_t 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_query_task.navigation_layers & connection.polygon->owner->navigation_layers) == 0) {
|
||||
if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const gd::NavigationPoly &least_cost_poly = navigation_polys[p_query_task.least_cost_id];
|
||||
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->travel_cost;
|
||||
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->owner_rid != least_cost_poly.poly->owner->owner_rid) {
|
||||
poly_enter_cost = least_cost_poly.poly->owner->enter_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 = p_query_task.least_cost_id;
|
||||
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);
|
||||
|
|
@ -405,14 +337,14 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
// 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 = p_query_task.least_cost_id;
|
||||
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_position) *
|
||||
neighbor_poly.poly->owner->travel_cost;
|
||||
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.
|
||||
|
|
@ -421,14 +353,14 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
} else {
|
||||
// Initialize the matching navigation polygon.
|
||||
neighbor_poly.poly = connection.polygon;
|
||||
neighbor_poly.back_navigation_poly_id = p_query_task.least_cost_id;
|
||||
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_position) *
|
||||
neighbor_poly.poly->owner->travel_cost;
|
||||
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.
|
||||
|
|
@ -449,33 +381,37 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
}
|
||||
|
||||
// Set as end point the furthest reachable point.
|
||||
end_polygon = reachable_end;
|
||||
end_poly = reachable_end;
|
||||
real_t end_d = FLT_MAX;
|
||||
for (size_t point_id = 2; point_id < end_polygon->points.size(); point_id++) {
|
||||
Face3 f(end_polygon->points[0].pos, end_polygon->points[point_id - 1].pos, end_polygon->points[point_id].pos);
|
||||
Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
|
||||
real_t dpoint = spoint.distance_to(p_query_task.target_position);
|
||||
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_target_position);
|
||||
real_t dpoint = spoint.distance_to(p_target_position);
|
||||
if (dpoint < end_d) {
|
||||
end_position = spoint;
|
||||
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_polygon->points.size(); point_id++) {
|
||||
Face3 f(begin_polygon->points[0].pos, begin_polygon->points[point_id - 1].pos, begin_polygon->points[point_id].pos);
|
||||
Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
|
||||
real_t dpoint = spoint.distance_to(p_query_task.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_target_position);
|
||||
real_t dpoint = spoint.distance_to(p_target_position);
|
||||
if (dpoint < end_d) {
|
||||
end_position = spoint;
|
||||
end_point = spoint;
|
||||
end_d = dpoint;
|
||||
closest_point_on_start_poly = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (closest_point_on_start_poly) {
|
||||
_query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, end_polygon);
|
||||
// No point to run PostProcessing when start and end convex polygon is the same.
|
||||
p_query_task.path_clear();
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
|
||||
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
||||
return;
|
||||
}
|
||||
|
|
@ -483,9 +419,9 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
for (gd::NavigationPoly &nav_poly : navigation_polys) {
|
||||
nav_poly.poly = nullptr;
|
||||
}
|
||||
navigation_polys[begin_polygon->id].poly = begin_polygon;
|
||||
navigation_polys[begin_poly->id].poly = begin_poly;
|
||||
|
||||
p_query_task.least_cost_id = begin_polygon->id;
|
||||
least_cost_id = begin_poly->id;
|
||||
prev_least_cost_id = -1;
|
||||
|
||||
reachable_end = nullptr;
|
||||
|
|
@ -494,19 +430,19 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
}
|
||||
|
||||
// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
|
||||
p_query_task.least_cost_id = traversable_polys.pop()->poly->id;
|
||||
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[p_query_task.least_cost_id].entry.distance_to(p_query_task.target_position);
|
||||
real_t distance = navigation_polys[least_cost_id].entry.distance_to(p_target_position);
|
||||
if (distance_to_reachable_end > distance) {
|
||||
distance_to_reachable_end = distance;
|
||||
reachable_end = navigation_polys[p_query_task.least_cost_id].poly;
|
||||
reachable_end = navigation_polys[least_cost_id].poly;
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we reached the end
|
||||
if (navigation_polys[p_query_task.least_cost_id].poly == end_polygon) {
|
||||
if (navigation_polys[least_cost_id].poly == end_poly) {
|
||||
found_route = true;
|
||||
break;
|
||||
}
|
||||
|
|
@ -517,19 +453,29 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|||
if (!found_route) {
|
||||
real_t end_d = FLT_MAX;
|
||||
// Search all faces of the start polygon for the closest point to our target position.
|
||||
for (size_t point_id = 2; point_id < begin_polygon->points.size(); point_id++) {
|
||||
Face3 f(begin_polygon->points[0].pos, begin_polygon->points[point_id - 1].pos, begin_polygon->points[point_id].pos);
|
||||
Vector3 spoint = f.get_closest_point_to(p_query_task.target_position);
|
||||
real_t dpoint = spoint.distance_to(p_query_task.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_target_position);
|
||||
real_t dpoint = spoint.distance_to(p_target_position);
|
||||
if (dpoint < end_d) {
|
||||
end_position = spoint;
|
||||
end_point = spoint;
|
||||
end_d = dpoint;
|
||||
}
|
||||
}
|
||||
_query_task_create_same_polygon_two_point_path(p_query_task, begin_polygon, begin_polygon);
|
||||
|
||||
p_query_task.path_clear();
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
|
||||
p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
||||
return;
|
||||
}
|
||||
|
||||
p_query_task.end_position = end_point;
|
||||
p_query_task.end_polygon = end_poly;
|
||||
p_query_task.begin_position = begin_point;
|
||||
p_query_task.begin_polygon = begin_poly;
|
||||
p_query_task.least_cost_id = least_cost_id;
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) {
|
||||
|
|
@ -574,26 +520,63 @@ void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D
|
|||
}
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
|
||||
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_query_task.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_query_task.start_position);
|
||||
real_t distance_to_point = point.distance_to(p_query_task.start_position);
|
||||
if (distance_to_point < begin_d) {
|
||||
begin_d = distance_to_point;
|
||||
p_query_task.begin_polygon = &p;
|
||||
p_query_task.begin_position = point;
|
||||
}
|
||||
|
||||
point = face.get_closest_point_to(p_query_task.target_position);
|
||||
distance_to_point = point.distance_to(p_query_task.target_position);
|
||||
if (distance_to_point < end_d) {
|
||||
end_d = distance_to_point;
|
||||
p_query_task.end_polygon = &p;
|
||||
p_query_task.end_position = point;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task) {
|
||||
const Vector3 &begin_position = p_query_task.begin_position;
|
||||
const Vector3 &end_position = p_query_task.end_position;
|
||||
const Vector3 &map_up = p_query_task.map_up;
|
||||
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
|
||||
Vector3 end_point = p_query_task.end_position;
|
||||
const gd::Polygon *end_poly = p_query_task.end_polygon;
|
||||
Vector3 begin_point = p_query_task.begin_position;
|
||||
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
|
||||
uint32_t least_cost_id = p_query_task.least_cost_id;
|
||||
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
|
||||
Vector3 p_map_up = p_query_task.map_up;
|
||||
|
||||
// Set the apex poly/point to the end point
|
||||
gd::NavigationPoly *apex_poly = &p_path_corridor[p_query_task.least_cost_id];
|
||||
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_position, back_pathway);
|
||||
if (end_position.is_equal_approx(back_edge_closest_point)) {
|
||||
const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
|
||||
if (end_point.is_equal_approx(back_edge_closest_point)) {
|
||||
// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
|
||||
// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
|
||||
if (apex_poly->back_navigation_poly_id != -1) {
|
||||
apex_poly = &p_path_corridor[apex_poly->back_navigation_poly_id];
|
||||
apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 apex_point = end_position;
|
||||
Vector3 apex_point = end_point;
|
||||
|
||||
gd::NavigationPoly *left_poly = apex_poly;
|
||||
Vector3 left_portal = apex_point;
|
||||
|
|
@ -602,20 +585,20 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
|
|||
|
||||
gd::NavigationPoly *p = apex_poly;
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
|
||||
|
||||
while (p) {
|
||||
// Set left and right points of the pathway between polygons.
|
||||
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(map_up) < 0) {
|
||||
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(map_up) >= 0) {
|
||||
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(map_up) > 0) {
|
||||
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 {
|
||||
|
|
@ -629,14 +612,13 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
|
|||
right_portal = apex_point;
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
|
||||
|
||||
skip = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(map_up) <= 0) {
|
||||
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(map_up) < 0) {
|
||||
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 {
|
||||
|
|
@ -655,7 +637,7 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
|
|||
|
||||
// Go to the previous polygon.
|
||||
if (p->back_navigation_poly_id != -1) {
|
||||
p = &p_path_corridor[p->back_navigation_poly_id];
|
||||
p = &navigation_polys[p->back_navigation_poly_id];
|
||||
} else {
|
||||
// The end
|
||||
p = nullptr;
|
||||
|
|
@ -663,95 +645,59 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
|
|||
}
|
||||
|
||||
// If the last point is not the begin point, add it to the list.
|
||||
if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_position) {
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
|
||||
if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_point) {
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task) {
|
||||
const Vector3 &begin_position = p_query_task.begin_position;
|
||||
const Vector3 &end_position = p_query_task.end_position;
|
||||
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
|
||||
Vector3 end_point = p_query_task.end_position;
|
||||
const gd::Polygon *end_poly = p_query_task.end_polygon;
|
||||
Vector3 begin_point = p_query_task.begin_position;
|
||||
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
|
||||
uint32_t least_cost_id = p_query_task.least_cost_id;
|
||||
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
|
||||
|
||||
// Add mid points.
|
||||
int np_id = p_query_task.least_cost_id;
|
||||
while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
|
||||
if (p_path_corridor[np_id].back_navigation_edge != -1) {
|
||||
int prev = p_path_corridor[np_id].back_navigation_edge;
|
||||
int prev_n = (p_path_corridor[np_id].back_navigation_edge + 1) % p_path_corridor[np_id].poly->points.size();
|
||||
Vector3 point = (p_path_corridor[np_id].poly->points[prev].pos + p_path_corridor[np_id].poly->points[prev_n].pos) * 0.5;
|
||||
// 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;
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, point, p_path_corridor[np_id].poly);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, point, navigation_polys[np_id].poly);
|
||||
} else {
|
||||
_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
|
||||
}
|
||||
|
||||
np_id = p_path_corridor[np_id].back_navigation_poly_id;
|
||||
np_id = navigation_polys[np_id].back_navigation_poly_id;
|
||||
}
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task) {
|
||||
const Vector3 &begin_position = p_query_task.begin_position;
|
||||
const Vector3 &end_position = p_query_task.end_position;
|
||||
LocalVector<gd::NavigationPoly> &p_path_corridor = p_query_task.path_query_slot->path_corridor;
|
||||
Vector3 end_point = p_query_task.end_position;
|
||||
const gd::Polygon *end_poly = p_query_task.end_polygon;
|
||||
Vector3 begin_point = p_query_task.begin_position;
|
||||
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
|
||||
uint32_t least_cost_id = p_query_task.least_cost_id;
|
||||
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_position, p_query_task.end_polygon);
|
||||
_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
|
||||
|
||||
// Add mid points.
|
||||
int np_id = p_query_task.least_cost_id;
|
||||
while (np_id != -1 && p_path_corridor[np_id].back_navigation_poly_id != -1) {
|
||||
_query_task_push_back_point_with_metadata(p_query_task, p_path_corridor[np_id].entry, p_path_corridor[np_id].poly);
|
||||
// Add mid points
|
||||
int np_id = least_cost_id;
|
||||
while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
|
||||
_query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
|
||||
|
||||
np_id = p_path_corridor[np_id].back_navigation_poly_id;
|
||||
np_id = navigation_polys[np_id].back_navigation_poly_id;
|
||||
}
|
||||
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_position, p_query_task.begin_polygon);
|
||||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
|
||||
// Find begin polyon and begin position closest to start position and
|
||||
// end polyon and end position closest to target position on the map.
|
||||
real_t begin_d = FLT_MAX;
|
||||
real_t end_d = FLT_MAX;
|
||||
|
||||
Vector3 begin_position;
|
||||
Vector3 end_position;
|
||||
|
||||
// Find the initial poly and the end poly on this map.
|
||||
for (const gd::Polygon &polygon : p_polygons) {
|
||||
// Only consider the polygon if it in a region with compatible layers.
|
||||
if ((p_query_task.navigation_layers & polygon.owner->navigation_layers) == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// For each face check the distance between the origin/destination.
|
||||
for (size_t point_id = 2; point_id < polygon.points.size(); point_id++) {
|
||||
const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);
|
||||
|
||||
Vector3 point = face.get_closest_point_to(p_query_task.start_position);
|
||||
real_t distance_to_point = point.distance_to(p_query_task.start_position);
|
||||
if (distance_to_point < begin_d) {
|
||||
begin_d = distance_to_point;
|
||||
p_query_task.begin_polygon = &polygon;
|
||||
begin_position = point;
|
||||
}
|
||||
|
||||
point = face.get_closest_point_to(p_query_task.target_position);
|
||||
distance_to_point = point.distance_to(p_query_task.target_position);
|
||||
if (distance_to_point < end_d) {
|
||||
end_d = distance_to_point;
|
||||
p_query_task.end_polygon = &polygon;
|
||||
end_position = point;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
p_query_task.begin_position = begin_position;
|
||||
p_query_task.end_position = end_position;
|
||||
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
|
||||
}
|
||||
|
||||
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) {
|
||||
|
|
@ -878,7 +824,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co
|
|||
closest_point_distance_squared = distance_squared;
|
||||
result.point = p_point - plane_normalized * distance;
|
||||
result.normal = plane_normal;
|
||||
result.owner = polygon.owner->owner_rid;
|
||||
result.owner = polygon.owner->get_self();
|
||||
|
||||
if (Math::is_zero_approx(distance)) {
|
||||
break;
|
||||
|
|
@ -890,7 +836,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co
|
|||
closest_point_distance_squared = distance;
|
||||
result.point = closest_on_polygon;
|
||||
result.normal = plane_normal;
|
||||
result.owner = polygon.owner->owner_rid;
|
||||
result.owner = polygon.owner->get_self();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -904,16 +850,16 @@ RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Pol
|
|||
}
|
||||
|
||||
void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) {
|
||||
const Vector3 &map_up = p_query_task.map_up;
|
||||
LocalVector<gd::NavigationPoly> &path_corridor = p_query_task.path_query_slot->path_corridor;
|
||||
Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1];
|
||||
const LocalVector<gd::NavigationPoly> &p_navigation_polys = p_query_task.path_query_slot->path_corridor;
|
||||
const Vector3 p_map_up = p_query_task.map_up;
|
||||
|
||||
if (from.is_equal_approx(p_to_point)) {
|
||||
return;
|
||||
}
|
||||
|
||||
Plane cut_plane;
|
||||
cut_plane.normal = (from - p_to_point).cross(map_up);
|
||||
cut_plane.normal = (from - p_to_point).cross(p_map_up);
|
||||
if (cut_plane.normal == Vector3()) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -925,7 +871,7 @@ void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_tas
|
|||
Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
|
||||
|
||||
ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
|
||||
from_poly = &path_corridor[from_poly->back_navigation_poly_id];
|
||||
from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
|
||||
|
||||
if (!pathway_start.is_equal_approx(pathway_end)) {
|
||||
Vector3 inters;
|
||||
|
|
|
|||
|
|
@ -82,7 +82,6 @@ public:
|
|||
Vector3 map_up;
|
||||
NavMap *map = nullptr;
|
||||
PathQuerySlot *path_query_slot = nullptr;
|
||||
uint32_t link_polygons_size = 0;
|
||||
|
||||
// Path points.
|
||||
LocalVector<Vector3> path_points;
|
||||
|
|
@ -94,6 +93,20 @@ public:
|
|||
Ref<NavigationPathQueryResult3D> query_result;
|
||||
Callable callback;
|
||||
NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED;
|
||||
|
||||
void path_clear() {
|
||||
path_points.clear();
|
||||
path_meta_point_types.clear();
|
||||
path_meta_point_rids.clear();
|
||||
path_meta_point_owners.clear();
|
||||
}
|
||||
|
||||
void path_reverse() {
|
||||
path_points.invert();
|
||||
path_meta_point_types.invert();
|
||||
path_meta_point_rids.invert();
|
||||
path_meta_point_owners.invert();
|
||||
}
|
||||
};
|
||||
|
||||
static bool emit_callback(const Callable &p_callback);
|
||||
|
|
@ -109,7 +122,6 @@ public:
|
|||
static void map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
|
||||
|
||||
static void query_task_polygons_get_path(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
|
||||
static void _query_task_create_same_polygon_two_point_path(NavMeshPathQueryTask3D &p_query_task, const gd::Polygon *p_begin_polygon, const gd::Polygon *p_end_polygon);
|
||||
static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon);
|
||||
static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
|
||||
static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons);
|
||||
|
|
|
|||
|
|
@ -41,6 +41,11 @@ struct NavRegionIteration : NavBaseIteration {
|
|||
LocalVector<gd::Polygon> navmesh_polygons;
|
||||
real_t surface_area = 0.0;
|
||||
AABB bounds;
|
||||
|
||||
const Transform3D &get_transform() const { return transform; }
|
||||
const LocalVector<gd::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
|
||||
real_t get_surface_area() const { return surface_area; }
|
||||
AABB get_bounds() const { return bounds; }
|
||||
};
|
||||
|
||||
#endif // NAV_REGION_ITERATION_3D_H
|
||||
|
|
|
|||
|
|
@ -129,8 +129,10 @@ void NavLink::get_iteration_update(NavLinkIteration &r_iteration) {
|
|||
r_iteration.travel_cost = get_travel_cost();
|
||||
r_iteration.owner_object_id = get_owner_id();
|
||||
r_iteration.owner_type = get_type();
|
||||
r_iteration.owner_rid = get_self();
|
||||
|
||||
r_iteration.enabled = enabled;
|
||||
r_iteration.start_position = start_position;
|
||||
r_iteration.end_position = end_position;
|
||||
r_iteration.enabled = get_enabled();
|
||||
r_iteration.start_position = get_start_position();
|
||||
r_iteration.end_position = get_end_position();
|
||||
r_iteration.bidirectional = is_bidirectional();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -40,6 +40,10 @@ struct NavLinkIteration : NavBaseIteration {
|
|||
Vector3 start_position;
|
||||
Vector3 end_position;
|
||||
LocalVector<gd::Polygon> navmesh_polygons;
|
||||
|
||||
Vector3 get_start_position() const { return start_position; }
|
||||
Vector3 get_end_position() const { return end_position; }
|
||||
bool is_bidirectional() const { return bidirectional; }
|
||||
};
|
||||
|
||||
#include "core/templates/self_list.h"
|
||||
|
|
|
|||
|
|
@ -165,7 +165,6 @@ void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task)
|
|||
}
|
||||
|
||||
p_query_task.map_up = map_iteration.map_up;
|
||||
p_query_task.link_polygons_size = map_iteration.link_polygon_count;
|
||||
|
||||
NavMeshQueries3D::query_task_polygons_get_path(p_query_task, map_iteration.navmesh_polygons);
|
||||
|
||||
|
|
|
|||
|
|
@ -250,51 +250,21 @@ void NavRegion::get_iteration_update(NavRegionIteration &r_iteration) {
|
|||
r_iteration.travel_cost = get_travel_cost();
|
||||
r_iteration.owner_object_id = get_owner_id();
|
||||
r_iteration.owner_type = get_type();
|
||||
|
||||
r_iteration.enabled = enabled;
|
||||
r_iteration.transform = transform;
|
||||
r_iteration.owner_use_edge_connections = use_edge_connections;
|
||||
r_iteration.bounds = get_bounds();
|
||||
|
||||
r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
|
||||
|
||||
for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
|
||||
const gd::Polygon &from_polygon = navmesh_polygons[i];
|
||||
gd::Polygon &to_polygon = r_iteration.navmesh_polygons[i];
|
||||
|
||||
to_polygon.surface_area = from_polygon.surface_area;
|
||||
to_polygon.owner = &r_iteration;
|
||||
to_polygon.points.resize(from_polygon.points.size());
|
||||
|
||||
const LocalVector<gd::Point> &from_points = from_polygon.points;
|
||||
LocalVector<gd::Point> &to_points = to_polygon.points;
|
||||
|
||||
to_points.resize(from_points.size());
|
||||
|
||||
for (uint32_t j = 0; j < from_points.size(); j++) {
|
||||
to_points[j].pos = from_points[j].pos;
|
||||
to_points[j].key = from_points[j].key;
|
||||
}
|
||||
|
||||
const LocalVector<gd::Edge> &from_edges = from_polygon.edges;
|
||||
LocalVector<gd::Edge> &to_edges = to_polygon.edges;
|
||||
|
||||
to_edges.resize(from_edges.size());
|
||||
|
||||
for (uint32_t j = 0; j < from_edges.size(); j++) {
|
||||
const LocalVector<gd::Edge::Connection> &from_connections = from_edges[j].connections;
|
||||
LocalVector<gd::Edge::Connection> &to_connections = to_edges[j].connections;
|
||||
|
||||
to_connections.resize(from_connections.size());
|
||||
|
||||
for (uint32_t k = 0; k < from_connections.size(); k++) {
|
||||
to_connections[k] = from_connections[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
r_iteration.surface_area = surface_area;
|
||||
r_iteration.owner_rid = get_self();
|
||||
|
||||
r_iteration.enabled = get_enabled();
|
||||
r_iteration.transform = get_transform();
|
||||
r_iteration.owner_use_edge_connections = get_use_edge_connections();
|
||||
r_iteration.bounds = get_bounds();
|
||||
r_iteration.surface_area = get_surface_area();
|
||||
|
||||
r_iteration.navmesh_polygons.clear();
|
||||
r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
|
||||
for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
|
||||
gd::Polygon &navmesh_polygon = navmesh_polygons[i];
|
||||
navmesh_polygon.owner = &r_iteration;
|
||||
r_iteration.navmesh_polygons[i] = navmesh_polygon;
|
||||
}
|
||||
}
|
||||
|
||||
void NavRegion::request_sync() {
|
||||
|
|
|
|||
|
|
@ -69,6 +69,47 @@ void NavigationPathQueryResult3D::reset() {
|
|||
path_owner_ids.clear();
|
||||
}
|
||||
|
||||
void NavigationPathQueryResult3D::set_data(const LocalVector<Vector3> &p_path, const LocalVector<int32_t> &p_path_types, const LocalVector<RID> &p_path_rids, const LocalVector<int64_t> &p_path_owner_ids) {
|
||||
path.clear();
|
||||
path_types.clear();
|
||||
path_rids.clear();
|
||||
path_owner_ids.clear();
|
||||
|
||||
{
|
||||
path.resize(p_path.size());
|
||||
Vector3 *w = path.ptrw();
|
||||
const Vector3 *r = p_path.ptr();
|
||||
for (uint32_t i = 0; i < p_path.size(); i++) {
|
||||
w[i] = r[i];
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
path_types.resize(p_path_types.size());
|
||||
int32_t *w = path_types.ptrw();
|
||||
const int32_t *r = p_path_types.ptr();
|
||||
for (uint32_t i = 0; i < p_path_types.size(); i++) {
|
||||
w[i] = r[i];
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
path_rids.resize(p_path_rids.size());
|
||||
for (uint32_t i = 0; i < p_path_rids.size(); i++) {
|
||||
path_rids[i] = p_path_rids[i];
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
path_owner_ids.resize(p_path_owner_ids.size());
|
||||
int64_t *w = path_owner_ids.ptrw();
|
||||
const int64_t *r = p_path_owner_ids.ptr();
|
||||
for (uint32_t i = 0; i < p_path_owner_ids.size(); i++) {
|
||||
w[i] = r[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavigationPathQueryResult3D::_bind_methods() {
|
||||
ClassDB::bind_method(D_METHOD("set_path", "path"), &NavigationPathQueryResult3D::set_path);
|
||||
ClassDB::bind_method(D_METHOD("get_path"), &NavigationPathQueryResult3D::get_path);
|
||||
|
|
|
|||
|
|
@ -65,6 +65,8 @@ public:
|
|||
const Vector<int64_t> &get_path_owner_ids() const;
|
||||
|
||||
void reset();
|
||||
|
||||
void set_data(const LocalVector<Vector3> &p_path, const LocalVector<int32_t> &p_path_types, const LocalVector<RID> &p_path_rids, const LocalVector<int64_t> &p_path_owner_ids);
|
||||
};
|
||||
|
||||
VARIANT_ENUM_CAST(NavigationPathQueryResult3D::PathSegmentType);
|
||||
|
|
|
|||
Loading…
Reference in a new issue