Fix navmesh not finding optimal paths

Addresses part of godotengine#17885

scene/3d/navigation.cpp

@@ -340,16 +340,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 			};
 
 			Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
-			begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
+			begin_poly->edges[i].C->distance = begin_point.distance_to(entry);
 			begin_poly->edges[i].C->entry = entry;
 #else
 			begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
 #endif
 			open_list.push_back(begin_poly->edges[i].C);
-
-			if (begin_poly->edges[i].C == end_poly) {
-				found_route = true;
-			}
 		}
 	}
 
@@ -370,28 +366,7 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 
 			float cost = p->distance;
 #ifdef USE_ENTRY_POINT
-			int es = p->edges.size();
-
-			float shortest_distance = 1e30;
-
-			for (int i = 0; i < es; i++) {
-				Polygon::Edge &e = p->edges.write[i];
-
-				if (!e.C)
-					continue;
-
-				Vector3 edge[2] = {
-					_get_vertex(p->edges[i].point),
-					_get_vertex(p->edges[(i + 1) % es].point)
-				};
-
-				Vector3 edge_point = Geometry::get_closest_point_to_segment(p->entry, edge);
-				float dist = p->entry.distance_to(edge_point);
-				if (dist < shortest_distance)
-					shortest_distance = dist;
-			}
-
-			cost += shortest_distance;
+			cost += p->entry.distance_to(end_point);
 #else
 			cost += p->center.distance_to(end_point);
 #endif
@@ -404,14 +379,30 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 		Polygon *p = least_cost_poly->get();
 		//open the neighbours for search
 
+		if (p == end_poly) {
+			//oh my reached end! stop algorithm
+			found_route = true;
+			break;
+		}
+
 		for (int i = 0; i < p->edges.size(); i++) {
 
 			Polygon::Edge &e = p->edges.write[i];
 
 			if (!e.C)
 				continue;
 
+#ifdef USE_ENTRY_POINT
+			Vector3 edge[2] = {
+				_get_vertex(p->edges[i].point),
+				_get_vertex(p->edges[(i + 1) % p->edges.size()].point)
+			};
+
+			Vector3 entry = Geometry::get_closest_point_to_segment(p->entry, edge);
+			float distance = p->entry.distance_to(entry) + p->distance;
+#else
 			float distance = p->center.distance_to(e.C->center) + p->distance;
+#endif
 
 			if (e.C->prev_edge != -1) {
 				//oh this was visited already, can we win the cost?
@@ -420,25 +411,22 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 
 					e.C->prev_edge = e.C_edge;
 					e.C->distance = distance;
+#ifdef USE_ENTRY_POINT
+					e.C->entry = entry;
+#endif
 				}
 			} else {
 				//add to open neighbours
 
 				e.C->prev_edge = e.C_edge;
 				e.C->distance = distance;
+#ifdef USE_ENTRY_POINT
+				e.C->entry = entry;
+#endif
 				open_list.push_back(e.C);
-
-				if (e.C == end_poly) {
-					//oh my reached end! stop algorithm
-					found_route = true;
-					break;
-				}
 			}
 		}
 
-		if (found_route)
-			break;
-
 		open_list.erase(least_cost_poly);
 	}
 
@@ -539,8 +527,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 			path.push_back(end_point);
 			while (true) {
 				int prev = p->prev_edge;
+#ifdef USE_ENTRY_POINT
+				Vector3 point = p->entry;
+#else
 				int prev_n = (p->prev_edge + 1) % p->edges.size();
 				Vector3 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
+#endif
 				path.push_back(point);
 				p = p->edges[prev].C;
 				if (p == begin_poly)