1 files changed, 26 insertions, 13 deletions

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index e3b048fd74..1b789bab9d 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -121,7 +121,6 @@ void Navigation2D::_navpoly_link(int p_id) {
 					pending.edge = j;
 					p.edges.write[j].P = C->get().pending.push_back(pending);
 					continue;
-					//print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b));
 				}
 
 				C->get().B = &p;
@@ -144,8 +143,6 @@ void Navigation2D::_navpoly_unlink(int p_id) {
 	NavMesh &nm = navpoly_map[p_id];
 	ERR_FAIL_COND(!nm.linked);
 
-	//print_line("UNLINK");
-
 	for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
 
 		Polygon &p = E->get();
@@ -341,7 +338,6 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 		path.resize(2);
 		path.write[0] = begin_point;
 		path.write[1] = end_point;
-		//print_line("Direct Path");
 		return path;
 	}
 
@@ -379,7 +375,6 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 	while (!found_route) {
 
 		if (open_list.size() == 0) {
-			//print_line("NOU OPEN LIST");
 			break;
 		}
 		//check open list
@@ -393,10 +388,34 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 			Polygon *p = E->get();
 
 			float cost = p->distance;
-			cost += p->center.distance_to(end_point);
 
-			if (cost < least_cost) {
+#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;
 
+				Vector2 edge[2] = {
+					_get_vertex(p->edges[i].point),
+					_get_vertex(p->edges[(i + 1) % es].point)
+				};
+
+				Vector2 edge_point = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
+				float dist = p->entry.distance_to(edge_point);
+				if (dist < shortest_distance)
+					shortest_distance = dist;
+			}
+
+			cost += shortest_distance;
+#else
+			cost += p->center.distance_to(end_point);
+#endif
+			if (cost < least_cost) {
 				least_cost_poly = E;
 				least_cost = cost;
 			}
@@ -526,7 +545,6 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 					if (portal_left.distance_squared_to(apex_point) < CMP_EPSILON || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
 						left_poly = p;
 						portal_left = left;
-						//print_line("***ADVANCE LEFT");
 					} else {
 
 						apex_point = portal_right;
@@ -537,8 +555,6 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 						if (!path.size() || path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
 							path.push_back(apex_point);
 						skip = true;
-						//print_line("addpoint left");
-						//print_line("***CLIP LEFT");
 					}
 				}
 
@@ -547,7 +563,6 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 					if (portal_right.distance_squared_to(apex_point) < CMP_EPSILON || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
 						right_poly = p;
 						portal_right = right;
-						//print_line("***ADVANCE RIGHT");
 					} else {
 
 						apex_point = portal_left;
@@ -557,8 +572,6 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 						portal_left = apex_point;
 						if (!path.size() || path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
 							path.push_back(apex_point);
-						//print_line("addpoint right");
-						//print_line("***CLIP RIGHT");
 					}
 				}