seedlingattempt/godot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Replace NavigationServer3D Point struct with Vector3

Browse Source 
Replaces NavigationServer3D Point struct with Vector3.
This commit is contained in:
smix8
2025-04-10 01:03:48 +02:00
parent 06c71fbf40
commit af14aaecd4
4 changed files with 70 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
modules/navigation_3d/3d/nav_map_builder_3d.cpp
View File

@ -107,7 +107,7 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
PerformanceData &performance_data = r_build.performance_data;
NavMapIteration3D *map_iteration = r_build.map_iteration;
int polygon_count = r_build.polygon_count;
const Vector3 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
// Group all edges per key.
@ -121,9 +121,9 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
}
for (Polygon &poly : region.navmesh_polygons) {
for (uint32_t p = 0; p < poly.points.size(); p++) {
const int next_point = (p + 1) % poly.points.size();
const EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
for (uint32_t p = 0; p < poly.vertices.size(); p++) {
const int next_point = (p + 1) % poly.vertices.size();
const EdgeKey ek(get_point_key(poly.vertices[p], merge_rasterizer_cell_size), get_point_key(poly.vertices[next_point], merge_rasterizer_cell_size));
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
if (!pair_it) {
@ -137,8 +137,8 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
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;
new_connection.pathway_start = poly.vertices[p];
new_connection.pathway_end = poly.vertices[next_point];
pair.connections[pair.size] = new_connection;
++pair.size;
@ -208,8 +208,8 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
for (uint32_t i = 0; i < free_edges.size(); i++) {
const 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_p1 = free_edge.polygon->vertices[free_edge.edge];
Vector3 edge_p2 = free_edge.polygon->vertices[(free_edge.edge + 1) % free_edge.polygon->vertices.size()];
for (uint32_t j = 0; j < free_edges.size(); j++) {
const Edge::Connection &other_edge = free_edges[j];
@ -217,8 +217,8 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
continue;
}
Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
Vector3 other_edge_p1 = other_edge.polygon->vertices[other_edge.edge];
Vector3 other_edge_p2 = other_edge.polygon->vertices[(other_edge.edge + 1) % other_edge.polygon->vertices.size()];
// Compute the projection of the opposite edge on the current one
Vector3 edge_vector = edge_p2 - edge_p1;
@ -268,7 +268,6 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
NavMapIteration3D *map_iteration = r_build.map_iteration;
real_t link_connection_radius = r_build.link_connection_radius;
Vector3 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
LocalVector<Polygon> &link_polygons = map_iteration->link_polygons;
LocalVector<NavLinkIteration3D> &links = map_iteration->link_iterations;
@ -304,8 +303,8 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
}
for (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 (uint32_t point_id = 2; point_id < polyon.vertices.size(); point_id += 1) {
const Face3 face(polyon.vertices[0], polyon.vertices[point_id - 1], polyon.vertices[point_id]);
{
const Vector3 start_point = face.get_closest_point_to(link_start_pos);
@ -342,28 +341,28 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
new_polygon.edges.clear();
new_polygon.edges.resize(4);
new_polygon.points.resize(4);
new_polygon.vertices.resize(4);
// Build a set of vertices that create a thin polygon going from the start to the end point.
new_polygon.points[0] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
new_polygon.points[1] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
new_polygon.points[2] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
new_polygon.points[3] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
new_polygon.vertices[0] = closest_start_point;
new_polygon.vertices[1] = closest_start_point;
new_polygon.vertices[2] = closest_end_point;
new_polygon.vertices[3] = closest_end_point;
// Setup connections to go forward in the link.
{
Edge::Connection entry_connection;
entry_connection.polygon = &new_polygon;
entry_connection.edge = -1;
entry_connection.pathway_start = new_polygon.points[0].pos;
entry_connection.pathway_end = new_polygon.points[1].pos;
entry_connection.pathway_start = new_polygon.vertices[0];
entry_connection.pathway_end = new_polygon.vertices[1];
closest_start_polygon->edges[0].connections.push_back(entry_connection);
Edge::Connection exit_connection;
exit_connection.polygon = closest_end_polygon;
exit_connection.edge = -1;
exit_connection.pathway_start = new_polygon.points[2].pos;
exit_connection.pathway_end = new_polygon.points[3].pos;
exit_connection.pathway_start = new_polygon.vertices[2];
exit_connection.pathway_end = new_polygon.vertices[3];
new_polygon.edges[2].connections.push_back(exit_connection);
}
@ -372,15 +371,15 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_
Edge::Connection entry_connection;
entry_connection.polygon = &new_polygon;
entry_connection.edge = -1;
entry_connection.pathway_start = new_polygon.points[2].pos;
entry_connection.pathway_end = new_polygon.points[3].pos;
entry_connection.pathway_start = new_polygon.vertices[2];
entry_connection.pathway_end = new_polygon.vertices[3];
closest_end_polygon->edges[0].connections.push_back(entry_connection);
Edge::Connection exit_connection;
exit_connection.polygon = closest_start_polygon;
exit_connection.edge = -1;
exit_connection.pathway_start = new_polygon.points[0].pos;
exit_connection.pathway_end = new_polygon.points[1].pos;
exit_connection.pathway_start = new_polygon.vertices[0];
exit_connection.pathway_end = new_polygon.vertices[1];
new_polygon.edges[0].connections.push_back(exit_connection);
}
}

84
modules/navigation_3d/3d/nav_mesh_queries_3d.cpp
View File

@ -89,8 +89,8 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<Polygon> &
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();
for (uint32_t rpp_index = 2; rpp_index < rr_polygon.vertices.size(); rpp_index++) {
real_t face_area = Face3(rr_polygon.vertices[0], rr_polygon.vertices[rpp_index - 1], rr_polygon.vertices[rpp_index]).get_area();
if (face_area == 0.0) {
continue;
@ -108,9 +108,9 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<Polygon> &
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());
ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.vertices.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);
const Face3 face(rr_polygon.vertices[0], rr_polygon.vertices[rrp_face_index - 1], rr_polygon.vertices[rrp_face_index]);
Vector3 face_random_position = face.get_random_point_inside();
return face_random_position;
@ -120,9 +120,9 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<Polygon> &
const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1);
uint32_t rrp_face_index = Math::random(int(2), rr_polygon.vertices.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);
const Face3 face(rr_polygon.vertices[0], rr_polygon.vertices[rrp_face_index - 1], rr_polygon.vertices[rrp_face_index]);
Vector3 face_random_position = face.get_random_point_inside();
return face_random_position;
@ -255,8 +255,8 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask
}
// For each face check the distance between the origin/destination.
for (uint32_t point_id = 2; point_id < p.points.size(); point_id++) {
const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
for (uint32_t point_id = 2; point_id < p.vertices.size(); point_id++) {
const Face3 face(p.vertices[0], p.vertices[point_id - 1], p.vertices[point_id]);
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);
@ -370,8 +370,8 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
// Set as end point the furthest reachable point.
end_poly = reachable_end;
real_t end_d = FLT_MAX;
for (uint32_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
for (uint32_t point_id = 2; point_id < end_poly->vertices.size(); point_id++) {
Face3 f(end_poly->vertices[0], end_poly->vertices[point_id - 1], end_poly->vertices[point_id]);
Vector3 spoint = f.get_closest_point_to(p_target_position);
real_t dpoint = spoint.distance_squared_to(p_target_position);
if (dpoint < end_d) {
@ -382,8 +382,8 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
// Search all faces of start polygon as well.
bool closest_point_on_start_poly = false;
for (uint32_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) {
Face3 f(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]);
Vector3 spoint = f.get_closest_point_to(p_target_position);
real_t dpoint = spoint.distance_squared_to(p_target_position);
if (dpoint < end_d) {
@ -441,8 +441,8 @@ 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 (uint32_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);
for (uint32_t point_id = 2; point_id < begin_poly->vertices.size(); point_id++) {
Face3 f(begin_poly->vertices[0], begin_poly->vertices[point_id - 1], begin_poly->vertices[point_id]);
Vector3 spoint = f.get_closest_point_to(p_target_position);
real_t dpoint = spoint.distance_squared_to(p_target_position);
if (dpoint < end_d) {
@ -682,8 +682,8 @@ void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTas
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;
int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->vertices.size();
Vector3 point = (navigation_polys[np_id].poly->vertices[prev] + navigation_polys[np_id].poly->vertices[prev_n]) * 0.5;
_query_task_push_back_point_with_metadata(p_query_task, point, navigation_polys[np_id].poly);
} else {
@ -726,8 +726,8 @@ Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMa
for (const NavRegionIteration3D &region : regions) {
for (const Polygon &polygon : region.get_navmesh_polygons()) {
// For each face check the distance to the segment.
for (uint32_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);
for (uint32_t point_id = 2; point_id < polygon.vertices.size(); point_id += 1) {
const Face3 face(polygon.vertices[0], polygon.vertices[point_id - 1], polygon.vertices[point_id]);
Vector3 intersection_point;
if (face.intersects_segment(p_from, p_to, &intersection_point)) {
const real_t d = p_from.distance_to(intersection_point);
@ -759,14 +759,14 @@ Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMa
}
// 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 (uint32_t point_id = 0; point_id < polygon.points.size(); point_id += 1) {
for (uint32_t point_id = 0; point_id < polygon.vertices.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,
polygon.vertices[point_id],
polygon.vertices[(point_id + 1) % polygon.vertices.size()],
a,
b);
@ -805,13 +805,13 @@ ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(c
const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
for (const NavRegionIteration3D &region : regions) {
for (const Polygon &polygon : region.get_navmesh_polygons()) {
Vector3 plane_normal = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
Vector3 plane_normal = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]);
Vector3 closest_on_polygon;
real_t closest = FLT_MAX;
bool inside = true;
Vector3 previous = polygon.points[polygon.points.size() - 1].pos;
for (uint32_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
Vector3 edge = polygon.points[point_id].pos - previous;
Vector3 previous = polygon.vertices[polygon.vertices.size() - 1];
for (uint32_t point_id = 0; point_id < polygon.vertices.size(); ++point_id) {
Vector3 edge = polygon.vertices[point_id] - previous;
Vector3 to_point = p_point - previous;
Vector3 edge_to_point_pormal = edge.cross(to_point);
bool clockwise = edge_to_point_pormal.dot(plane_normal) > 0;
@ -822,9 +822,9 @@ ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(c
real_t edge_square = edge.length_squared();
if (point_projected_on_edge > edge_square) {
real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
real_t distance = polygon.vertices[point_id].distance_squared_to(p_point);
if (distance < closest) {
closest_on_polygon = polygon.points[point_id].pos;
closest_on_polygon = polygon.vertices[point_id];
closest = distance;
}
} else if (point_projected_on_edge < 0.f) {
@ -840,12 +840,12 @@ ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(c
break;
}
}
previous = polygon.points[point_id].pos;
previous = polygon.vertices[point_id];
}
if (inside) {
Vector3 plane_normalized = plane_normal.normalized();
real_t distance = plane_normalized.dot(p_point - polygon.points[0].pos);
real_t distance = plane_normalized.dot(p_point - polygon.vertices[0]);
real_t distance_squared = distance * distance;
if (distance_squared < closest_point_distance_squared) {
closest_point_distance_squared = distance_squared;
@ -941,8 +941,8 @@ Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVecto
for (const Polygon &polygon : p_polygons) {
// For each face check the distance to the segment.
for (uint32_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);
for (uint32_t point_id = 2; point_id < polygon.vertices.size(); point_id += 1) {
const Face3 face(polygon.vertices[0], polygon.vertices[point_id - 1], polygon.vertices[point_id]);
Vector3 intersection_point;
if (face.intersects_segment(p_from, p_to, &intersection_point)) {
const real_t d = p_from.distance_to(intersection_point);
@ -974,14 +974,14 @@ Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVecto
}
// 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 (uint32_t point_id = 0; point_id < polygon.points.size(); point_id += 1) {
for (uint32_t point_id = 0; point_id < polygon.vertices.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,
polygon.vertices[point_id],
polygon.vertices[(point_id + 1) % polygon.vertices.size()],
a,
b);
@ -1012,13 +1012,13 @@ ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const
real_t closest_point_distance_squared = FLT_MAX;
for (const Polygon &polygon : p_polygons) {
Vector3 plane_normal = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
Vector3 plane_normal = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]);
Vector3 closest_on_polygon;
real_t closest = FLT_MAX;
bool inside = true;
Vector3 previous = polygon.points[polygon.points.size() - 1].pos;
for (uint32_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
Vector3 edge = polygon.points[point_id].pos - previous;
Vector3 previous = polygon.vertices[polygon.vertices.size() - 1];
for (uint32_t point_id = 0; point_id < polygon.vertices.size(); ++point_id) {
Vector3 edge = polygon.vertices[point_id] - previous;
Vector3 to_point = p_point - previous;
Vector3 edge_to_point_pormal = edge.cross(to_point);
bool clockwise = edge_to_point_pormal.dot(plane_normal) > 0;
@ -1029,9 +1029,9 @@ ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const
real_t edge_square = edge.length_squared();
if (point_projected_on_edge > edge_square) {
real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
real_t distance = polygon.vertices[point_id].distance_squared_to(p_point);
if (distance < closest) {
closest_on_polygon = polygon.points[point_id].pos;
closest_on_polygon = polygon.vertices[point_id];
closest = distance;
}
} else if (point_projected_on_edge < 0.f) {
@ -1047,12 +1047,12 @@ ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const
break;
}
}
previous = polygon.points[point_id].pos;
previous = polygon.vertices[point_id];
}
if (inside) {
Vector3 plane_normalized = plane_normal.normalized();
real_t distance = plane_normalized.dot(p_point - polygon.points[0].pos);
real_t distance = plane_normalized.dot(p_point - polygon.vertices[0]);
real_t distance_squared = distance * distance;
if (distance_squared < closest_point_distance_squared) {
closest_point_distance_squared = distance_squared;

5
modules/navigation_3d/nav_region_3d.cpp
View File

@ -245,7 +245,7 @@ void NavRegion3D::update_polygons() {
const int *indices = navigation_mesh_polygon.ptr();
bool valid(true);
polygon.points.resize(navigation_mesh_polygon_size);
polygon.vertices.resize(navigation_mesh_polygon_size);
polygon.edges.resize(navigation_mesh_polygon_size);
real_t _new_polygon_surface_area = 0.0;
@ -270,8 +270,7 @@ void NavRegion3D::update_polygons() {
}
Vector3 point_position = transform.xform(vertices_r[idx]);
polygon.points[j].pos = point_position;
polygon.points[j].key = NavMapBuilder3D::get_point_key(point_position, map->get_merge_rasterizer_cell_size());
polygon.vertices[j] = point_position;
if (first_vertex) {
first_vertex = false;

8
modules/navigation_3d/nav_utils_3d.h
View File

@ -72,11 +72,6 @@ struct EdgeKey {
}
};
struct Point {
Vector3 pos;
PointKey key;
};
struct Edge {
/// The gateway in the edge, as, in some case, the whole edge might not be navigable.
struct Connection {
@ -104,8 +99,7 @@ struct Polygon {
/// Navigation region or link that contains this polygon.
const NavBaseIteration3D *owner = nullptr;
/// The points of this `Polygon`
LocalVector<Point> points;
LocalVector<Vector3> vertices;
/// The edges of this `Polygon`
LocalVector<Edge> edges;