NavMap Fix polygons being treated like triangle strips instead of triangle fans

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