|
|
@@ -206,72 +206,49 @@ 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_clear();
|
|
|
-
|
|
|
- _query_task_find_start_end_positions(p_query_task, p_polygons);
|
|
|
-
|
|
|
- // Check for trivial cases.
|
|
|
- if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
|
|
|
- p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
|
|
- return;
|
|
|
- }
|
|
|
- if (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;
|
|
|
- }
|
|
|
-
|
|
|
- _query_task_build_path_corridor(p_query_task, p_polygons);
|
|
|
+void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) {
|
|
|
+ real_t begin_d = FLT_MAX;
|
|
|
+ real_t end_d = FLT_MAX;
|
|
|
|
|
|
- if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
|
|
|
- return;
|
|
|
- }
|
|
|
+ const LocalVector<NavRegionIteration> ®ions = p_map_iteration.region_iterations;
|
|
|
|
|
|
- // Post-Process path.
|
|
|
- switch (p_query_task.path_postprocessing) {
|
|
|
- case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
|
|
|
- _query_task_post_process_corridorfunnel(p_query_task);
|
|
|
- } break;
|
|
|
- case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
|
|
|
- _query_task_post_process_edgecentered(p_query_task);
|
|
|
- } break;
|
|
|
- case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
|
|
|
- _query_task_post_process_nopostprocessing(p_query_task);
|
|
|
- } break;
|
|
|
- default: {
|
|
|
- WARN_PRINT("No match for used PathPostProcessing - fallback to default");
|
|
|
- _query_task_post_process_corridorfunnel(p_query_task);
|
|
|
- } break;
|
|
|
- }
|
|
|
-
|
|
|
- p_query_task.path_reverse();
|
|
|
+ for (const NavRegionIteration ®ion : regions) {
|
|
|
+ if (!region.get_enabled()) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
|
|
|
- if (p_query_task.simplify_path) {
|
|
|
- _query_task_simplified_path_points(p_query_task);
|
|
|
- }
|
|
|
+ // Find the initial poly and the end poly on this map.
|
|
|
+ for (const gd::Polygon &p : region.get_navmesh_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;
|
|
|
+ }
|
|
|
|
|
|
-#ifdef DEBUG_ENABLED
|
|
|
- // Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
|
|
|
- if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
|
|
|
- DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
|
|
|
- }
|
|
|
+ // 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);
|
|
|
|
|
|
- if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
|
|
|
- DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
|
|
|
- }
|
|
|
+ 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;
|
|
|
+ }
|
|
|
|
|
|
- if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
|
|
|
- DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|
|
|
-#endif // DEBUG_ENABLED
|
|
|
-
|
|
|
- p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
|
|
}
|
|
|
|
|
|
-void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const LocalVector<gd::Polygon> &p_polygons) {
|
|
|
+void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task) {
|
|
|
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;
|
|
|
@@ -478,6 +455,71 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
|
|
|
p_query_task.least_cost_id = least_cost_id;
|
|
|
}
|
|
|
|
|
|
+void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) {
|
|
|
+ p_query_task.path_clear();
|
|
|
+
|
|
|
+ _query_task_find_start_end_positions(p_query_task, p_map_iteration);
|
|
|
+
|
|
|
+ // Check for trivial cases.
|
|
|
+ if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
|
|
|
+ p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (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;
|
|
|
+ }
|
|
|
+
|
|
|
+ _query_task_build_path_corridor(p_query_task);
|
|
|
+
|
|
|
+ if (p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask3D::TaskStatus::QUERY_FAILED) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Post-Process path.
|
|
|
+ switch (p_query_task.path_postprocessing) {
|
|
|
+ case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
|
|
|
+ _query_task_post_process_corridorfunnel(p_query_task);
|
|
|
+ } break;
|
|
|
+ case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
|
|
|
+ _query_task_post_process_edgecentered(p_query_task);
|
|
|
+ } break;
|
|
|
+ case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
|
|
|
+ _query_task_post_process_nopostprocessing(p_query_task);
|
|
|
+ } break;
|
|
|
+ default: {
|
|
|
+ WARN_PRINT("No match for used PathPostProcessing - fallback to default");
|
|
|
+ _query_task_post_process_corridorfunnel(p_query_task);
|
|
|
+ } break;
|
|
|
+ }
|
|
|
+
|
|
|
+ p_query_task.path_reverse();
|
|
|
+
|
|
|
+ if (p_query_task.simplify_path) {
|
|
|
+ _query_task_simplified_path_points(p_query_task);
|
|
|
+ }
|
|
|
+
|
|
|
+#ifdef DEBUG_ENABLED
|
|
|
+ // Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
|
|
|
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
|
|
|
+ DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
|
|
|
+ }
|
|
|
+
|
|
|
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
|
|
|
+ DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
|
|
|
+ }
|
|
|
+
|
|
|
+ if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
|
|
|
+ DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
|
|
|
+ }
|
|
|
+#endif // DEBUG_ENABLED
|
|
|
+
|
|
|
+ p_query_task.status = NavMeshPathQueryTask3D::TaskStatus::QUERY_FINISHED;
|
|
|
+}
|
|
|
+
|
|
|
void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task) {
|
|
|
if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) {
|
|
|
return;
|
|
|
@@ -520,40 +562,6 @@ 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) {
|
|
|
Vector3 end_point = p_query_task.end_position;
|
|
|
const gd::Polygon *end_poly = p_query_task.end_polygon;
|
|
|
@@ -700,6 +708,223 @@ void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQuer
|
|
|
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
|
|
|
}
|
|
|
|
|
|
+Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMapIteration &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
|
|
|
+ bool use_collision = p_use_collision;
|
|
|
+ Vector3 closest_point;
|
|
|
+ real_t closest_point_distance = FLT_MAX;
|
|
|
+
|
|
|
+ const LocalVector<NavRegionIteration> ®ions = p_map_iteration.region_iterations;
|
|
|
+ for (const NavRegionIteration ®ion : regions) {
|
|
|
+ for (const gd::Polygon &polygon : region.get_navmesh_polygons()) {
|
|
|
+ // For each face check the distance to the segment.
|
|
|
+ for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) {
|
|
|
+ const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);
|
|
|
+ Vector3 intersection_point;
|
|
|
+ if (face.intersects_segment(p_from, p_to, &intersection_point)) {
|
|
|
+ const real_t d = p_from.distance_to(intersection_point);
|
|
|
+ if (!use_collision) {
|
|
|
+ closest_point = intersection_point;
|
|
|
+ use_collision = true;
|
|
|
+ closest_point_distance = d;
|
|
|
+ } else if (closest_point_distance > d) {
|
|
|
+ closest_point = intersection_point;
|
|
|
+ closest_point_distance = d;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // If segment does not itersect face, check the distance from segment's endpoints.
|
|
|
+ else if (!use_collision) {
|
|
|
+ const Vector3 p_from_closest = face.get_closest_point_to(p_from);
|
|
|
+ const real_t d_p_from = p_from.distance_to(p_from_closest);
|
|
|
+ if (closest_point_distance > d_p_from) {
|
|
|
+ closest_point = p_from_closest;
|
|
|
+ closest_point_distance = d_p_from;
|
|
|
+ }
|
|
|
+
|
|
|
+ const Vector3 p_to_closest = face.get_closest_point_to(p_to);
|
|
|
+ const real_t d_p_to = p_to.distance_to(p_to_closest);
|
|
|
+ if (closest_point_distance > d_p_to) {
|
|
|
+ closest_point = p_to_closest;
|
|
|
+ closest_point_distance = d_p_to;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ // Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment.
|
|
|
+ if (!use_collision) {
|
|
|
+ for (size_t point_id = 0; point_id < polygon.points.size(); point_id += 1) {
|
|
|
+ Vector3 a, b;
|
|
|
+
|
|
|
+ Geometry3D::get_closest_points_between_segments(
|
|
|
+ p_from,
|
|
|
+ p_to,
|
|
|
+ polygon.points[point_id].pos,
|
|
|
+ polygon.points[(point_id + 1) % polygon.points.size()].pos,
|
|
|
+ a,
|
|
|
+ b);
|
|
|
+
|
|
|
+ const real_t d = a.distance_to(b);
|
|
|
+ if (d < closest_point_distance) {
|
|
|
+ closest_point_distance = d;
|
|
|
+ closest_point = b;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return closest_point;
|
|
|
+}
|
|
|
+
|
|
|
+Vector3 NavMeshQueries3D::map_iteration_get_closest_point(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
|
|
|
+ gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
|
|
|
+ return cp.point;
|
|
|
+}
|
|
|
+
|
|
|
+Vector3 NavMeshQueries3D::map_iteration_get_closest_point_normal(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
|
|
|
+ gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
|
|
|
+ return cp.normal;
|
|
|
+}
|
|
|
+
|
|
|
+RID NavMeshQueries3D::map_iteration_get_closest_point_owner(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
|
|
|
+ gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
|
|
|
+ return cp.owner;
|
|
|
+}
|
|
|
+
|
|
|
+gd::ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
|
|
|
+ gd::ClosestPointQueryResult result;
|
|
|
+ real_t closest_point_distance_squared = FLT_MAX;
|
|
|
+
|
|
|
+ const LocalVector<NavRegionIteration> ®ions = p_map_iteration.region_iterations;
|
|
|
+ for (const NavRegionIteration ®ion : regions) {
|
|
|
+ for (const gd::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 closest_on_polygon;
|
|
|
+ real_t closest = FLT_MAX;
|
|
|
+ bool inside = true;
|
|
|
+ Vector3 previous = polygon.points[polygon.points.size() - 1].pos;
|
|
|
+ for (size_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
|
|
|
+ Vector3 edge = polygon.points[point_id].pos - 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;
|
|
|
+ // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge.
|
|
|
+ if (!clockwise) {
|
|
|
+ inside = false;
|
|
|
+ real_t point_projected_on_edge = edge.dot(to_point);
|
|
|
+ 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);
|
|
|
+ if (distance < closest) {
|
|
|
+ closest_on_polygon = polygon.points[point_id].pos;
|
|
|
+ closest = distance;
|
|
|
+ }
|
|
|
+ } else if (point_projected_on_edge < 0.f) {
|
|
|
+ real_t distance = previous.distance_squared_to(p_point);
|
|
|
+ if (distance < closest) {
|
|
|
+ closest_on_polygon = previous;
|
|
|
+ closest = distance;
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+ // If we project on this edge, this will be the closest point.
|
|
|
+ real_t percent = point_projected_on_edge / edge_square;
|
|
|
+ closest_on_polygon = previous + percent * edge;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ previous = polygon.points[point_id].pos;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (inside) {
|
|
|
+ Vector3 plane_normalized = plane_normal.normalized();
|
|
|
+ real_t distance = plane_normalized.dot(p_point - polygon.points[0].pos);
|
|
|
+ real_t distance_squared = distance * distance;
|
|
|
+ if (distance_squared < closest_point_distance_squared) {
|
|
|
+ closest_point_distance_squared = distance_squared;
|
|
|
+ result.point = p_point - plane_normalized * distance;
|
|
|
+ result.normal = plane_normal;
|
|
|
+ result.owner = polygon.owner->get_self();
|
|
|
+
|
|
|
+ if (Math::is_zero_approx(distance)) {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ } else {
|
|
|
+ real_t distance = closest_on_polygon.distance_squared_to(p_point);
|
|
|
+ if (distance < closest_point_distance_squared) {
|
|
|
+ closest_point_distance_squared = distance;
|
|
|
+ result.point = closest_on_polygon;
|
|
|
+ result.normal = plane_normal;
|
|
|
+ result.owner = polygon.owner->get_self();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) {
|
|
|
+ if (p_map_iteration.region_iterations.is_empty()) {
|
|
|
+ return Vector3();
|
|
|
+ }
|
|
|
+
|
|
|
+ LocalVector<uint32_t> accessible_regions;
|
|
|
+ accessible_regions.reserve(p_map_iteration.region_iterations.size());
|
|
|
+
|
|
|
+ for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) {
|
|
|
+ const NavRegionIteration ®ion = p_map_iteration.region_iterations[i];
|
|
|
+ if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ accessible_regions.push_back(i);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (accessible_regions.is_empty()) {
|
|
|
+ // All existing region polygons are disabled.
|
|
|
+ return Vector3();
|
|
|
+ }
|
|
|
+
|
|
|
+ if (p_uniformly) {
|
|
|
+ real_t accumulated_region_surface_area = 0;
|
|
|
+ RBMap<real_t, uint32_t> accessible_regions_area_map;
|
|
|
+
|
|
|
+ for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) {
|
|
|
+ const NavRegionIteration ®ion = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
|
|
|
+
|
|
|
+ real_t region_surface_area = region.surface_area;
|
|
|
+
|
|
|
+ if (region_surface_area == 0.0f) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index;
|
|
|
+ accumulated_region_surface_area += region_surface_area;
|
|
|
+ }
|
|
|
+ if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) {
|
|
|
+ // All faces have no real surface / no area.
|
|
|
+ return Vector3();
|
|
|
+ }
|
|
|
+
|
|
|
+ real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area);
|
|
|
+
|
|
|
+ RBMap<real_t, uint32_t>::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map);
|
|
|
+ ERR_FAIL_COND_V(!E, Vector3());
|
|
|
+ uint32_t random_region_index = E->value;
|
|
|
+ ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
|
|
|
+
|
|
|
+ const NavRegionIteration &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
|
|
|
+
|
|
|
+ return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
|
|
|
+
|
|
|
+ } else {
|
|
|
+ uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
|
|
|
+
|
|
|
+ const NavRegionIteration &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
|
|
|
+
|
|
|
+ return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
|
|
|
bool use_collision = p_use_collision;
|
|
|
Vector3 closest_point;
|
smix8