diff options
author | kleonc <9283098+kleonc@users.noreply.github.com> | 2022-02-13 16:07:01 +0100 |
---|---|---|
committer | kleonc <9283098+kleonc@users.noreply.github.com> | 2022-02-13 19:26:17 +0100 |
commit | 5422d863e175d92b9092a43dd6c9107843b1b943 (patch) | |
tree | 606ea9e3e6ea31a2fdfbf05b959b289d72d12660 | |
parent | 48ed0400bc2313b9652d7f6cfd6119d784adc956 (diff) |
NavMap Fix polygons being treated like triangle strips instead of triangle fans
-rw-r--r-- | modules/navigation/nav_map.cpp | 94 | ||||
-rw-r--r-- | modules/navigation/nav_map.h | 1 | ||||
-rw-r--r-- | modules/navigation/nav_utils.h | 6 |
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 |