Better heuristic for the shortest path algorithm for navigation2D and navigation.

Better heuristic for the shortest path algorithm for navigation2D and navigation.
It now will use the shortest distance to the polygon as cost instead of the distance to the center.

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;
+
+#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;
 			}

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;
+#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;
+#else
 			cost += p->center.distance_to(end_point);
-
+#endif
 			if (cost < least_cost) {
-
 				least_cost_poly = E;
 				least_cost = cost;
 			}

scene/3d/navigation.h

@@ -94,6 +94,7 @@ class Navigation : public Spatial {
 		Vector<Edge> edges;
 
 		Vector3 center;
+		Vector3 entry;
 
 		float distance;
 		int prev_edge;