3 files changed, 65 insertions, 4 deletions

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index 9eec8e6cc3..1b789bab9d 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -388,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;
 			}
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 8d84d2408c..54f74c2df3 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -30,6 +30,8 @@
 
 #include "navigation.h"
 
+#define USE_ENTRY_POINT
+
 void Navigation::_navmesh_link(int p_id) {
 
 	ERR_FAIL_COND(!navmesh_map.has(p_id));
@@ -331,7 +333,18 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 		if (begin_poly->edges[i].C) {
 
 			begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
+#ifdef USE_ENTRY_POINT
+			Vector3 edge[2] = {
+				_get_vertex(begin_poly->edges[i].point),
+				_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
+			};
+
+			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->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) {
@@ -356,10 +369,33 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 			Polygon *p = E->get();
 
 			float cost = p->distance;
-			cost += p->center.distance_to(end_point);
+#ifdef USE_ENTRY_POINT
+			int es = p->edges.size();
 
-			if (cost < least_cost) {
+			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;
+#else
+			cost += p->center.distance_to(end_point);
+#endif
+			if (cost < least_cost) {
 				least_cost_poly = E;
 				least_cost = cost;
 			}
diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h
index 5a501039c8..8f200997cd 100644
--- a/scene/3d/navigation.h
+++ b/scene/3d/navigation.h
@@ -94,6 +94,7 @@ class Navigation : public Spatial {
 		Vector<Edge> edges;
 
 		Vector3 center;
+		Vector3 entry;
 
 		float distance;
 		int prev_edge;