summaryrefslogtreecommitdiff
path: root/modules
diff options
context:
space:
mode:
Diffstat (limited to 'modules')
-rw-r--r--modules/navigation/nav_map.cpp94
-rw-r--r--modules/navigation/nav_map.h1
-rw-r--r--modules/navigation/nav_utils.h6
3 files changed, 32 insertions, 69 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 76c31a5f42..1151df6390 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -83,12 +83,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
continue;
}
- // For each point cast a face and check the distance between the origin/destination
- for (size_t point_id = 0; point_id < p.points.size(); point_id++) {
- const Vector3 p1 = p.points[point_id].pos;
- const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
- const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
- const Face3 face(p1, p2, p3);
+ // For each face check the distance between the origin/destination
+ for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+ const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 point = face.get_closest_point_to(p_origin);
float distance_to_point = point.distance_to(p_origin);
@@ -214,7 +211,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
end_poly = reachable_end;
end_d = 1e20;
for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
- Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+ Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
Vector3 spoint = f.get_closest_point_to(p_destination);
float dpoint = spoint.distance_to(p_destination);
if (dpoint < end_d) {
@@ -370,13 +367,12 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
Vector3 closest_point;
real_t closest_point_d = 1e20;
- // Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i];
- // For each point cast a face and check the distance to the segment
+ // For each face check the distance to the segment
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
Vector3 inters;
if (f.intersects_segment(p_from, p_to, &inters)) {
const real_t d = closest_point_d = p_from.distance_to(inters);
@@ -416,82 +412,42 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
}
Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
- // TODO this is really not optimal, please redesign the API to directly return all this data
-
- Vector3 closest_point;
- real_t closest_point_d = 1e20;
-
- // Find the initial poly and the end poly on this map.
- for (size_t i(0); i < polygons.size(); i++) {
- const gd::Polygon &p = polygons[i];
-
- // For each point cast a face and check the distance to the point
- for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
- const Vector3 inters = f.get_closest_point_to(p_point);
- const real_t d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_d = d;
- }
- }
- }
-
- return closest_point;
+ gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+ return cp.point;
}
Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
- // TODO this is really not optimal, please redesign the API to directly return all this data
-
- Vector3 closest_point;
- Vector3 closest_point_normal;
- real_t closest_point_d = 1e20;
-
- // Find the initial poly and the end poly on this map.
- for (size_t i(0); i < polygons.size(); i++) {
- const gd::Polygon &p = polygons[i];
-
- // For each point cast a face and check the distance to the point
- for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
- const Vector3 inters = f.get_closest_point_to(p_point);
- const real_t d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_normal = f.get_plane().normal;
- closest_point_d = d;
- }
- }
- }
-
- return closest_point_normal;
+ gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+ return cp.normal;
}
RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
- // TODO this is really not optimal, please redesign the API to directly return all this data
+ gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+ return cp.owner;
+}
- Vector3 closest_point;
- RID closest_point_owner;
- real_t closest_point_d = 1e20;
+gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
+ gd::ClosestPointQueryResult result;
+ real_t closest_point_ds = 1e20;
- // Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i];
- // For each point cast a face and check the distance to the point
+ // For each face check the distance to the point
for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
- const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
const Vector3 inters = f.get_closest_point_to(p_point);
- const real_t d = inters.distance_to(p_point);
- if (d < closest_point_d) {
- closest_point = inters;
- closest_point_owner = p.owner->get_self();
- closest_point_d = d;
+ const real_t ds = inters.distance_squared_to(p_point);
+ if (ds < closest_point_ds) {
+ result.point = inters;
+ result.normal = f.get_plane().normal;
+ result.owner = p.owner->get_self();
+ closest_point_ds = ds;
}
}
}
- return closest_point_owner;
+ return result;
}
void NavMap::add_region(NavRegion *p_region) {
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index 1802f4e907..f46297a7ce 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -104,6 +104,7 @@ public:
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
Vector3 get_closest_point(const Vector3 &p_point) const;
Vector3 get_closest_point_normal(const Vector3 &p_point) const;
+ gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
RID get_closest_point_owner(const Vector3 &p_point) const;
void add_region(NavRegion *p_region);
diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h
index a6f51a4698..30930ed6ae 100644
--- a/modules/navigation/nav_utils.h
+++ b/modules/navigation/nav_utils.h
@@ -131,6 +131,12 @@ struct NavigationPoly {
}
};
+struct ClosestPointQueryResult {
+ Vector3 point;
+ Vector3 normal;
+ RID owner;
+};
+
} // namespace gd
#endif // NAV_UTILS_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-27 22:20:29 +0000