diff options
Diffstat (limited to 'modules/navigation/3d/godot_navigation_server_3d.cpp')
| -rw-r--r-- | modules/navigation/3d/godot_navigation_server_3d.cpp | 151 |
1 files changed, 151 insertions, 0 deletions
diff --git a/modules/navigation/3d/godot_navigation_server_3d.cpp b/modules/navigation/3d/godot_navigation_server_3d.cpp index 15de33f58f..61a128e004 100644 --- a/modules/navigation/3d/godot_navigation_server_3d.cpp +++ b/modules/navigation/3d/godot_navigation_server_3d.cpp @@ -1202,6 +1202,11 @@ COMMAND_1(free, RID, p_object) { } else if (obstacle_owner.owns(p_object)) { internal_free_obstacle(p_object); +#ifndef _3D_DISABLED + } else if (navmesh_generator_3d && navmesh_generator_3d->owns(p_object)) { + navmesh_generator_3d->free(p_object); +#endif // _3D_DISABLED + } else { ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed)."); } @@ -1381,11 +1386,157 @@ PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters & // add path postprocessing + if (r_query_result.path.size() > 2 && p_parameters.simplify_path) { + const LocalVector<uint32_t> &simplified_path_indices = get_simplified_path_indices(r_query_result.path, p_parameters.simplify_epsilon); + + uint32_t indices_count = simplified_path_indices.size(); + + { + Vector3 *w = r_query_result.path.ptrw(); + const Vector3 *r = r_query_result.path.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path.resize(indices_count); + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) { + int32_t *w = r_query_result.path_types.ptrw(); + const int32_t *r = r_query_result.path_types.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path_types.resize(indices_count); + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) { + TypedArray<RID> simplified_path_rids; + simplified_path_rids.resize(indices_count); + for (uint32_t i = 0; i < indices_count; i++) { + simplified_path_rids[i] = r_query_result.path_rids[i]; + } + r_query_result.path_rids = simplified_path_rids; + } + + if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) { + int64_t *w = r_query_result.path_owner_ids.ptrw(); + const int64_t *r = r_query_result.path_owner_ids.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + r_query_result.path_owner_ids.resize(indices_count); + } + } + // add path stats return r_query_result; } +RID GodotNavigationServer3D::source_geometry_parser_create() { +#ifndef _3D_DISABLED + if (navmesh_generator_3d) { + return navmesh_generator_3d->source_geometry_parser_create(); + } +#endif // _3D_DISABLED + return RID(); +} + +void GodotNavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) { +#ifndef _3D_DISABLED + if (navmesh_generator_3d) { + navmesh_generator_3d->source_geometry_parser_set_callback(p_parser, p_callback); + } +#endif // _3D_DISABLED +} + +Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) { + if (p_path.size() <= 2) { + return p_path; + } + + p_epsilon = MAX(0.0, p_epsilon); + + LocalVector<uint32_t> simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon); + + uint32_t indices_count = simplified_path_indices.size(); + + Vector<Vector3> simplified_path; + simplified_path.resize(indices_count); + + Vector3 *w = simplified_path.ptrw(); + const Vector3 *r = p_path.ptr(); + for (uint32_t i = 0; i < indices_count; i++) { + w[i] = r[simplified_path_indices[i]]; + } + + return simplified_path; +} + +LocalVector<uint32_t> GodotNavigationServer3D::get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon) { + p_epsilon = MAX(0.0, p_epsilon); + real_t squared_epsilon = p_epsilon * p_epsilon; + + LocalVector<bool> valid_points; + valid_points.resize(p_path.size()); + for (uint32_t i = 0; i < valid_points.size(); i++) { + valid_points[i] = false; + } + + simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points); + + int valid_point_index = 0; + + for (bool valid : valid_points) { + if (valid) { + valid_point_index += 1; + } + } + + LocalVector<uint32_t> simplified_path_indices; + simplified_path_indices.resize(valid_point_index); + valid_point_index = 0; + + for (uint32_t i = 0; i < valid_points.size(); i++) { + if (valid_points[i]) { + simplified_path_indices[valid_point_index] = i; + valid_point_index += 1; + } + } + + return simplified_path_indices; +} + +void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) { + r_valid_points[p_start_inx] = true; + r_valid_points[p_end_inx] = true; + + const Vector3 &start_point = p_points[p_start_inx]; + const Vector3 &end_point = p_points[p_end_inx]; + + Vector3 path_segment[2] = { start_point, end_point }; + + real_t point_max_distance = 0.0; + int point_max_index = 0; + + for (int i = p_start_inx; i < p_end_inx; i++) { + const Vector3 &checked_point = p_points[i]; + + const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment); + real_t distance_squared = closest_point.distance_squared_to(checked_point); + + if (distance_squared > point_max_distance) { + point_max_index = i; + point_max_distance = distance_squared; + } + } + + if (point_max_distance > p_epsilon) { + simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points); + simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points); + } +} + int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const { switch (p_info) { case INFO_ACTIVE_MAPS: { |